35#include "generated/modules.h"
36#include "generated/airframe.h"
43#ifndef CAM_GIMBAL_PAN0
44#define CAM_GIMBAL_PAN0 0
46#ifndef CAM_GIMBAL_TILT0
47#define CAM_GIMBAL_TILT0 0
52#ifndef CAM_GIMBAL_PAN_MAX
53#define CAM_GIMBAL_PAN_MAX RadOfDeg(90.f)
55#ifndef CAM_GIMBAL_PAN_MIN
56#define CAM_GIMBAL_PAN_MIN -CAM_GIMBAL_PAN_MAX
58#ifndef CAM_GIMBAL_TILT_MAX
59#define CAM_GIMBAL_TILT_MAX RadOfDeg(90.f)
61#ifndef CAM_GIMBAL_TILT_MIN
62#define CAM_GIMBAL_TILT_MIN -CAM_GIMBAL_TILT_MAX
66#ifndef CAM_GIMBAL_POS_X
67#define CAM_GIMBAL_POS_X 0.f
69#ifndef CAM_GIMBAL_POS_Y
70#define CAM_GIMBAL_POS_Y 0.f
72#ifndef CAM_GIMBAL_POS_Z
73#define CAM_GIMBAL_POS_Z 0.f
75#ifndef CAM_GIMBAL_TO_BODY_PHI
76#define CAM_GIMBAL_TO_BODY_PHI 0.f
78#ifndef CAM_GIMBAL_TO_BODY_THETA
79#define CAM_GIMBAL_TO_BODY_THETA 0.f
81#ifndef CAM_GIMBAL_TO_BODY_PSI
82#define CAM_GIMBAL_TO_BODY_PSI 0.f
97static void send_cam(
struct transport_tx *trans,
struct link_device *
dev)
103 pprz_msg_send_CAM(trans,
dev, AC_ID, &phi, &theta, &x, &y);
106#if CAM_SHOW_COORDINATES
107static void send_cam_point(
struct transport_tx *trans,
struct link_device *
dev)
114 pprz_msg_send_CAM_POINT(trans,
dev, AC_ID, &dist_from_home, &target_lla.lat, &target_lla.lon);
129 *pan = atan2f(
dir.y,
dir.x);
130 *tilt = asinf(-
dir.z);
212 float pan_max,
float pan_min,
213 float tilt_max,
float tilt_min)
348#ifdef CAM_SHOW_COORDINATES
360#ifdef COMMAND_CAM_PAN
361 command_set(COMMAND_CAM_PAN,
cam_gimbal.pan_cmd);
363#ifdef COMMAND_CAM_TILT
364 command_set(COMMAND_CAM_TILT,
cam_gimbal.tilt_cmd);
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ABI_BROADCAST
Broadcast address.
Core autopilot interface common to all firmwares.
void cam_gimbal_set_tilt_command(struct CamGimbal *cam, int16_t tilt)
void cam_gimbal_set_angles_deg(struct CamGimbal *cam, float pan, float tilt)
void cam_gimbal_run(struct CamGimbal *cam)
Run camera control.
void cam_gimbal_set_mode(struct CamGimbal *cam, uint8_t mode)
static void send_cam(struct transport_tx *trans, struct link_device *dev)
static void cam_gimbal_nadir(struct CamGimbal *cam)
Point straight down.
#define CAM_GIMBAL_TO_BODY_PHI
static void cam_gimbal_angles(struct CamGimbal *cam)
Computes the servo values from pan and tilt angles.
void cam_gimbal_init(void)
Init module.
void cam_gimbal_set_wp_id(struct CamGimbal *cam, uint8_t wp_id)
void cam_gimbal_set_lock(struct CamGimbal *cam, bool lock)
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 cam_gimbal_target(struct CamGimbal *cam)
Computes the right angles from target position.
void cam_gimbal_set_target_pos(struct CamGimbal *cam, struct EnuCoor_f target)
void cam_gimbal_set_ac_id(struct CamGimbal *cam, uint8_t ac_id)
static void cam_gimbal_joystick(struct CamGimbal *cam UNUSED)
#define CAM_GIMBAL_TO_BODY_PSI
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 g...
#define CAM_GIMBAL_TILT_MIN
void cam_gimbal_setup_angles(struct CamGimbal *cam, float pan_max, float pan_min, float tilt_max, float tilt_min)
#define CAM_GIMBAL_PAN_MIN
static abi_event joystick_ev
#define CAM_GIMBAL_PAN_MAX
void cam_gimbal_periodic(void)
Periodic call (run control).
static void cam_gimbal_waypoint_target(struct CamGimbal *cam)
#define CAM_GIMBAL_TO_BODY_THETA
#define CAM_GIMBAL_TILT_MAX
void cam_gimbal_set_angles_callback(struct CamGimbal *cam, cam_angles_from_dir compute_angles)
void cam_gimbal_setup_mounting(struct CamGimbal *cam, struct FloatEulers gimbal_to_body_eulers, struct FloatVect3 gimbal_pos)
void cam_gimbal_set_angles_rad(struct CamGimbal *cam, float pan, float tilt)
struct CamGimbal cam_gimbal
static void cam_gimbal_ac_target(struct CamGimbal *cam UNUSED)
void cam_gimbal_set_pan_command(struct CamGimbal *cam, int16_t pan)
Pan/Tilt camera gimbal control.
int16_t tilt_cmd
tilt command [pprz]
struct FloatRMat gimbal_to_body
rotation matrix from gimbal to body frame
#define CAM_GIMBAL_MODE_TARGET
float pan_min
pan angle at minimum command
float pan_angle
pan angle [rad]
#define CAM_GIMBAL_MODE_OFF
float pan_max
pan angle at maximum command
int16_t pan_cmd
pan command [pprz]
uint8_t target_wp_id
waypoint ID to track
#define CAM_GIMBAL_MODE_ANGLES
uint8_t mode
gimbal control mode
#define CAM_GIMBAL_MODE_JOYSTICK
#define CAM_GIMBAL_MODE_NB
float tilt_min
tilt angle at minimum command
bool lock
lock current command
uint8_t target_ac_id
aircraft ID to track
struct EnuCoor_f target_pos
target point in ENU world frame [m]
struct FloatVect3 gimbal_pos
position of the gimbal in body NED frame [m]
float tilt_angle
tilt angle [rad]
void(* cam_angles_from_dir)(struct FloatVect3 dir, float *pan, float *tilt)
Function pointer to return cam angle from a specified direction.
#define CAM_GIMBAL_MODE_NADIR
#define CAM_GIMBAL_MODE_AC_TARGET
float tilt_max
tilt angle at maximum command
cam_angles_from_dir compute_angles
cam angles from looking direction callback
#define CAM_GIMBAL_MODE_WAYPOINT
Hardware independent code for commands handling.
const uint8_t nb_waypoint
static struct uart_periph * dev
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
#define float_rmat_of_eulers
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
#define VECT3_SUB(_a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define ENU_OF_TO_NED(_po, _pi)
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.