Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
cam_gimbal.c File Reference

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.

Referenced by cam_gimbal_run().

◆ CAM_GIMBAL_PAN_MAX

#define CAM_GIMBAL_PAN_MAX   RadOfDeg(90.f)

Definition at line 53 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_PAN_MIN

#define CAM_GIMBAL_PAN_MIN   -CAM_GIMBAL_PAN_MAX

Definition at line 56 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_POS_X

#define CAM_GIMBAL_POS_X   0.f

Definition at line 67 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_POS_Y

#define CAM_GIMBAL_POS_Y   0.f

Definition at line 70 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_POS_Z

#define CAM_GIMBAL_POS_Z   0.f

Definition at line 73 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_TILT0

#define CAM_GIMBAL_TILT0   0

Definition at line 47 of file cam_gimbal.c.

Referenced by cam_gimbal_run().

◆ CAM_GIMBAL_TILT_MAX

#define CAM_GIMBAL_TILT_MAX   RadOfDeg(90.f)

Definition at line 59 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_TILT_MIN

#define CAM_GIMBAL_TILT_MIN   -CAM_GIMBAL_TILT_MAX

Definition at line 62 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_TO_BODY_PHI

#define CAM_GIMBAL_TO_BODY_PHI   0.f

Definition at line 76 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_TO_BODY_PSI

#define CAM_GIMBAL_TO_BODY_PSI   0.f

Definition at line 82 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

◆ CAM_GIMBAL_TO_BODY_THETA

#define CAM_GIMBAL_TO_BODY_THETA   0.f

Definition at line 79 of file cam_gimbal.c.

Referenced by cam_gimbal_init().

Function Documentation

◆ cam_gimbal_ac_target()

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(), target, and UNUSED.

Referenced by cam_gimbal_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ cam_gimbal_angles()

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 CamGimbal::lock, MAX_PPRZ, CamGimbal::pan_angle, CamGimbal::pan_cmd, CamGimbal::pan_max, CamGimbal::pan_min, CamGimbal::tilt_angle, CamGimbal::tilt_cmd, CamGimbal::tilt_max, and CamGimbal::tilt_min.

Referenced by cam_gimbal_run(), and cam_gimbal_target().

Here is the caller graph for this function:

◆ cam_gimbal_init()

◆ cam_gimbal_joystick()

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 UNUSED.

Referenced by cam_gimbal_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ cam_gimbal_nadir()

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(), 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, and cam_gimbal_run().

Here is the call graph for this function:

◆ cam_gimbal_run()

◆ 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 CamGimbal::target_ac_id.

◆ 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 CamGimbal::compute_angles.

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 )

◆ cam_gimbal_set_angles_rad()

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

◆ cam_gimbal_set_lock()

void cam_gimbal_set_lock ( struct CamGimbal * cam,
bool lock )

Definition at line 243 of file cam_gimbal.c.

References CamGimbal::lock.

◆ 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, CamGimbal::mode, 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 CamGimbal::pan_cmd, 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 target, and CamGimbal::target_pos.

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 CamGimbal::tilt_cmd, 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 nb_waypoint, and CamGimbal::target_wp_id.

◆ 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 CamGimbal::pan_max, CamGimbal::pan_min, CamGimbal::tilt_max, and CamGimbal::tilt_min.

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, CamGimbal::gimbal_pos, and CamGimbal::gimbal_to_body.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

◆ cam_gimbal_target()

void cam_gimbal_target ( struct CamGimbal * cam)
static

◆ cam_gimbal_waypoint_target()

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(), Min, nb_waypoint, stateGetPositionEnu_f(), target, CamGimbal::target_wp_id, 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()

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.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

◆ joystick_cb()

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, and UNUSED.

Referenced by cam_gimbal_init().

Here is the caller graph for this function:

◆ send_cam()

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

Definition at line 97 of file cam_gimbal.c.

References cam_gimbal, and dev.

Referenced by cam_gimbal_init(), and rotorcraft_cam_init().

Here is the caller graph for this function:

Variable Documentation

◆ cam_gimbal

struct CamGimbal cam_gimbal

◆ joystick_ev

abi_event joystick_ev
static

Definition at line 89 of file cam_gimbal.c.

Referenced by cam_gimbal_init().