Pan/Tilt camera gimbal control. More...

#include "cam_gimbal.h"
#include "autopilot.h"
#include "modules/nav/waypoints.h"
#include "generated/modules.h"
#include "generated/airframe.h"
#include "modules/core/commands.h"
#include "state.h"
#include "modules/core/abi.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for cam_gimbal.c:

Go to the source code of this file.

Macros
#define	CAM_GIMBAL_PAN0 0

#define	CAM_GIMBAL_TILT0 0

#define	CAM_GIMBAL_PAN_MAX RadOfDeg(90.f)

#define	CAM_GIMBAL_PAN_MIN -CAM_GIMBAL_PAN_MAX

#define	CAM_GIMBAL_TILT_MAX RadOfDeg(90.f)

#define	CAM_GIMBAL_TILT_MIN -CAM_GIMBAL_TILT_MAX

#define	CAM_GIMBAL_POS_X 0.f

#define	CAM_GIMBAL_POS_Y 0.f

#define	CAM_GIMBAL_POS_Z 0.f

#define	CAM_GIMBAL_TO_BODY_PHI 0.f

#define	CAM_GIMBAL_TO_BODY_THETA 0.f

#define	CAM_GIMBAL_TO_BODY_PSI 0.f

Functions
static void	joystick_cb (uint8_t sender_id UNUSED, int16_t roll, int16_t pitch, int16_t yaw UNUSED, int16_t throttle UNUSED)

static void	send_cam (struct transport_tx trans, struct link_device dev)

static void	default_compute_angles (struct FloatVect3 dir, float pan, float tilt)
	Default callback function to compute gimbal pan/tilt angle from a looking direction (unit vector in gimbal frame)

static void	cam_gimbal_angles (struct CamGimbal *cam)
	Computes the servo values from pan and tilt angles.

static void	cam_gimbal_target (struct CamGimbal *cam)
	Computes the right angles from target position.

static void	cam_gimbal_nadir (struct CamGimbal *cam)
	Point straight down.

static void	cam_gimbal_waypoint_target (struct CamGimbal *cam)

static void	cam_gimbal_ac_target (struct CamGimbal *cam UNUSED)

static void	cam_gimbal_joystick (struct CamGimbal *cam UNUSED)

void	cam_gimbal_setup_angles (struct CamGimbal *cam, float pan_max, float pan_min, float tilt_max, float tilt_min)

void	cam_gimbal_setup_mounting (struct CamGimbal *cam, struct FloatEulers gimbal_to_body_eulers, struct FloatVect3 gimbal_pos)

void	cam_gimbal_set_angles_callback (struct CamGimbal *cam, cam_angles_from_dir compute_angles)

void	cam_gimbal_set_mode (struct CamGimbal *cam, uint8_t mode)

void	cam_gimbal_set_lock (struct CamGimbal *cam, bool lock)

void	cam_gimbal_set_pan_command (struct CamGimbal *cam, int16_t pan)

void	cam_gimbal_set_tilt_command (struct CamGimbal *cam, int16_t tilt)

void	cam_gimbal_set_angles_rad (struct CamGimbal *cam, float pan, float tilt)

void	cam_gimbal_set_angles_deg (struct CamGimbal *cam, float pan, float tilt)

void	cam_gimbal_set_target_pos (struct CamGimbal *cam, struct EnuCoor_f target)

void	cam_gimbal_set_wp_id (struct CamGimbal *cam, uint8_t wp_id)

void	cam_gimbal_set_ac_id (struct CamGimbal *cam, uint8_t ac_id)

void	cam_gimbal_run (struct CamGimbal *cam)
	Run camera control.

void	cam_gimbal_init (void)
	Init module.

void	cam_gimbal_periodic (void)
	Periodic call (run control)

Variables
struct CamGimbal	cam_gimbal

static abi_event	joystick_ev

Detailed Description

Pan/Tilt camera gimbal control.

Definition in file cam_gimbal.c.

Macro Definition Documentation

◆ CAM_GIMBAL_PAN0

#define CAM_GIMBAL_PAN0 0

Definition at line 44 of file cam_gimbal.c.

◆ CAM_GIMBAL_PAN_MAX

#define CAM_GIMBAL_PAN_MAX RadOfDeg(90.f)

Definition at line 53 of file cam_gimbal.c.

◆ CAM_GIMBAL_PAN_MIN

#define CAM_GIMBAL_PAN_MIN -CAM_GIMBAL_PAN_MAX

Definition at line 56 of file cam_gimbal.c.

◆ CAM_GIMBAL_POS_X

#define CAM_GIMBAL_POS_X 0.f

Definition at line 67 of file cam_gimbal.c.

◆ CAM_GIMBAL_POS_Y

#define CAM_GIMBAL_POS_Y 0.f

Definition at line 70 of file cam_gimbal.c.

◆ CAM_GIMBAL_POS_Z

#define CAM_GIMBAL_POS_Z 0.f

Definition at line 73 of file cam_gimbal.c.

◆ CAM_GIMBAL_TILT0

#define CAM_GIMBAL_TILT0 0

Definition at line 47 of file cam_gimbal.c.

◆ CAM_GIMBAL_TILT_MAX

#define CAM_GIMBAL_TILT_MAX RadOfDeg(90.f)

Definition at line 59 of file cam_gimbal.c.

◆ CAM_GIMBAL_TILT_MIN

#define CAM_GIMBAL_TILT_MIN -CAM_GIMBAL_TILT_MAX

Definition at line 62 of file cam_gimbal.c.

◆ CAM_GIMBAL_TO_BODY_PHI

#define CAM_GIMBAL_TO_BODY_PHI 0.f

Definition at line 76 of file cam_gimbal.c.

◆ CAM_GIMBAL_TO_BODY_PSI

#define CAM_GIMBAL_TO_BODY_PSI 0.f

Definition at line 82 of file cam_gimbal.c.

◆ CAM_GIMBAL_TO_BODY_THETA

#define CAM_GIMBAL_TO_BODY_THETA 0.f

Definition at line 79 of file cam_gimbal.c.

Function Documentation

◆ cam_gimbal_ac_target()

static void cam_gimbal_ac_target ( struct CamGimbal *cam UNUSED )

static

Definition at line 197 of file cam_gimbal.c.

References acInfoGetPositionEnu_f(), cam_gimbal_target(), foo, and target.

Referenced by cam_gimbal_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cam_gimbal_angles()

static void cam_gimbal_angles ( struct CamGimbal * cam )

static

Computes the servo values from pan and tilt angles.

Definition at line 134 of file cam_gimbal.c.

References foo, and MAX_PPRZ.

Referenced by cam_gimbal_run(), and cam_gimbal_target().

Here is the caller graph for this function:

◆ cam_gimbal_init()

void cam_gimbal_init ( void )

Init module.

Definition at line 326 of file cam_gimbal.c.

References ABI_BROADCAST, cam_gimbal, CAM_GIMBAL_PAN_MAX, CAM_GIMBAL_PAN_MIN, CAM_GIMBAL_POS_X, CAM_GIMBAL_POS_Y, CAM_GIMBAL_POS_Z, cam_gimbal_set_angles_callback(), cam_gimbal_setup_angles(), cam_gimbal_setup_mounting(), CAM_GIMBAL_TILT_MAX, CAM_GIMBAL_TILT_MIN, CAM_GIMBAL_TO_BODY_PHI, CAM_GIMBAL_TO_BODY_PSI, CAM_GIMBAL_TO_BODY_THETA, default_compute_angles(), DefaultPeriodic, foo, joystick_cb(), joystick_ev, register_periodic_telemetry(), and send_cam().

Here is the call graph for this function:

◆ cam_gimbal_joystick()

static void cam_gimbal_joystick ( struct CamGimbal *cam UNUSED )

static

Definition at line 205 of file cam_gimbal.c.

References cam_gimbal_set_pan_command(), cam_gimbal_set_tilt_command(), and foo.

Referenced by cam_gimbal_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cam_gimbal_nadir()

static void cam_gimbal_nadir ( struct CamGimbal * cam )

static

Point straight down.

Definition at line 176 of file cam_gimbal.c.

References cam_gimbal_set_target_pos(), cam_gimbal_target(), foo, stateGetPositionEnu_f(), and target.

Referenced by cam_gimbal_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cam_gimbal_periodic()

void cam_gimbal_periodic ( void )

Periodic call (run control)

Definition at line 355 of file cam_gimbal.c.

References cam_gimbal, cam_gimbal_run(), foo, CamGimbal::pan_cmd, and CamGimbal::tilt_cmd.

Here is the call graph for this function:

◆ cam_gimbal_run()

void cam_gimbal_run ( struct CamGimbal * cam )

Run camera control.

Definition at line 294 of file cam_gimbal.c.

References cam_gimbal_ac_target(), cam_gimbal_angles(), cam_gimbal_joystick(), CAM_GIMBAL_MODE_AC_TARGET, CAM_GIMBAL_MODE_ANGLES, CAM_GIMBAL_MODE_JOYSTICK, CAM_GIMBAL_MODE_NADIR, CAM_GIMBAL_MODE_OFF, CAM_GIMBAL_MODE_TARGET, CAM_GIMBAL_MODE_WAYPOINT, cam_gimbal_nadir(), CAM_GIMBAL_PAN0, cam_gimbal_set_pan_command(), cam_gimbal_target(), CAM_GIMBAL_TILT0, cam_gimbal_waypoint_target(), and foo.

Referenced by cam_gimbal_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cam_gimbal_set_ac_id()

void cam_gimbal_set_ac_id	(	struct CamGimbal *	cam,
		uint8_t	ac_id
	)

Definition at line 286 of file cam_gimbal.c.

References foo.

◆ cam_gimbal_set_angles_callback()

void cam_gimbal_set_angles_callback	(	struct CamGimbal *	cam,
		cam_angles_from_dir	compute_angles
	)

Definition at line 229 of file cam_gimbal.c.

References foo.

Referenced by cam_gimbal_init(), and gimbal_caddx_gm3_init().

Here is the caller graph for this function:

◆ cam_gimbal_set_angles_deg()

void cam_gimbal_set_angles_deg	(	struct CamGimbal *	cam,
		float	pan,
		float	tilt
	)

Definition at line 266 of file cam_gimbal.c.

References foo.

◆ cam_gimbal_set_angles_rad()

void cam_gimbal_set_angles_rad	(	struct CamGimbal *	cam,
		float	pan,
		float	tilt
	)

Definition at line 258 of file cam_gimbal.c.

References foo.

◆ cam_gimbal_set_lock()

void cam_gimbal_set_lock	(	struct CamGimbal *	cam,
		bool	lock
	)

Definition at line 243 of file cam_gimbal.c.

References foo.

◆ cam_gimbal_set_mode()

void cam_gimbal_set_mode	(	struct CamGimbal *	cam,
		uint8_t	mode
	)

Definition at line 234 of file cam_gimbal.c.

References CAM_GIMBAL_MODE_NB, CAM_GIMBAL_MODE_OFF, foo, and mode.

◆ cam_gimbal_set_pan_command()

void cam_gimbal_set_pan_command	(	struct CamGimbal *	cam,
		int16_t	pan
	)

Definition at line 248 of file cam_gimbal.c.

References foo, and TRIM_PPRZ.

Referenced by cam_gimbal_joystick(), and cam_gimbal_run().

Here is the caller graph for this function:

◆ cam_gimbal_set_target_pos()

void cam_gimbal_set_target_pos	(	struct CamGimbal *	cam,
		struct EnuCoor_f	target
	)

Definition at line 274 of file cam_gimbal.c.

References foo, and target.

Referenced by cam_gimbal_nadir(), and cam_gimbal_waypoint_target().

Here is the caller graph for this function:

◆ cam_gimbal_set_tilt_command()

void cam_gimbal_set_tilt_command	(	struct CamGimbal *	cam,
		int16_t	tilt
	)

Definition at line 253 of file cam_gimbal.c.

References foo, and TRIM_PPRZ.

Referenced by cam_gimbal_joystick().

Here is the caller graph for this function:

◆ cam_gimbal_set_wp_id()

void cam_gimbal_set_wp_id	(	struct CamGimbal *	cam,
		uint8_t	wp_id
	)

Definition at line 279 of file cam_gimbal.c.

References foo, and nb_waypoint.

◆ cam_gimbal_setup_angles()

void cam_gimbal_setup_angles	(	struct CamGimbal *	cam,
		float	pan_max,
		float	pan_min,
		float	tilt_max,
		float	tilt_min
	)

Definition at line 211 of file cam_gimbal.c.

References foo.

Referenced by cam_gimbal_init(), and gimbal_caddx_gm3_init().

Here is the caller graph for this function:

◆ cam_gimbal_setup_mounting()

void cam_gimbal_setup_mounting	(	struct CamGimbal *	cam,
		struct FloatEulers	gimbal_to_body_eulers,
		struct FloatVect3	gimbal_pos
	)

Definition at line 221 of file cam_gimbal.c.

References float_rmat_of_eulers, and foo.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

◆ cam_gimbal_target()

static void cam_gimbal_target ( struct CamGimbal * cam )

static

Computes the right angles from target position.

Definition at line 148 of file cam_gimbal.c.

References cam_gimbal_angles(), ENU_OF_TO_NED, float_rmat_transp_vmult(), float_rmat_vmult(), float_vect3_normalize(), foo, stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), target, VECT3_DIFF, and VECT3_SUB.

Referenced by cam_gimbal_ac_target(), cam_gimbal_nadir(), cam_gimbal_run(), and cam_gimbal_waypoint_target().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cam_gimbal_waypoint_target()

static void cam_gimbal_waypoint_target ( struct CamGimbal * cam )

static

Definition at line 185 of file cam_gimbal.c.

References cam_gimbal_set_target_pos(), cam_gimbal_target(), foo, Min, nb_waypoint, stateGetPositionEnu_f(), target, WaypointX, WaypointY, and EnuCoor_f::z.

Referenced by cam_gimbal_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ default_compute_angles()

static void default_compute_angles	(	struct FloatVect3	dir,
		float *	pan,
		float *	tilt
	)

static

Default callback function to compute gimbal pan/tilt angle from a looking direction (unit vector in gimbal frame)

The default gimbal mounting is a pan angle turning around the gimbal z axis, then a tilt angle around the gimbal y axis. Therefor we have: -> tan(pan) = uy/ux -> sin(tilt) = -uz

Definition at line 127 of file cam_gimbal.c.

References dir, and foo.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

◆ joystick_cb()

static void joystick_cb	(	uint8_t sender_id	UNUSED,
		int16_t	roll,
		int16_t	pitch,
		int16_t yaw	UNUSED,
		int16_t throttle	UNUSED
	)

static

Definition at line 91 of file cam_gimbal.c.

References cam_gimbal, CamGimbal::pan_joystick, and CamGimbal::tilt_joystick.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

◆ send_cam()

static void send_cam	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 97 of file cam_gimbal.c.

References cam_gimbal, dev, foo, CamGimbal::pan_angle, CamGimbal::target_pos, CamGimbal::tilt_angle, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

Variable Documentation

◆ cam_gimbal

struct CamGimbal cam_gimbal

Definition at line 86 of file cam_gimbal.c.

Referenced by cam_gimbal_init(), cam_gimbal_periodic(), gimbal_caddx_gm3_init(), joystick_cb(), and send_cam().

◆ joystick_ev

abi_event joystick_ev

static

Definition at line 89 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ CAM_GIMBAL_PAN0

◆ CAM_GIMBAL_PAN_MAX

◆ CAM_GIMBAL_PAN_MIN

◆ CAM_GIMBAL_POS_X

◆ CAM_GIMBAL_POS_Y

◆ CAM_GIMBAL_POS_Z

◆ CAM_GIMBAL_TILT0

◆ CAM_GIMBAL_TILT_MAX

◆ CAM_GIMBAL_TILT_MIN

◆ CAM_GIMBAL_TO_BODY_PHI

◆ CAM_GIMBAL_TO_BODY_PSI

◆ CAM_GIMBAL_TO_BODY_THETA

Function Documentation

◆ cam_gimbal_ac_target()

◆ cam_gimbal_angles()

◆ cam_gimbal_init()

◆ cam_gimbal_joystick()

◆ cam_gimbal_nadir()

◆ cam_gimbal_periodic()

◆ cam_gimbal_run()

◆ cam_gimbal_set_ac_id()

◆ cam_gimbal_set_angles_callback()

◆ cam_gimbal_set_angles_deg()

◆ cam_gimbal_set_angles_rad()

◆ cam_gimbal_set_lock()

◆ cam_gimbal_set_mode()

◆ cam_gimbal_set_pan_command()

◆ cam_gimbal_set_target_pos()

◆ cam_gimbal_set_tilt_command()

◆ cam_gimbal_set_wp_id()

◆ cam_gimbal_setup_angles()

◆ cam_gimbal_setup_mounting()

◆ cam_gimbal_target()

◆ cam_gimbal_waypoint_target()

◆ default_compute_angles()

◆ joystick_cb()

◆ send_cam()

Variable Documentation

◆ cam_gimbal

◆ joystick_ev