Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[cam_gimbal] rewrite the pan/tilt control module (#3538)

Browse Source 
- generic pan/tilt gimbal control with several builtin modes
- gimbal position and orientation configurable
- specialization possible for gimbals not matching the default scheme
  (with an example using the Caddx GM3 3 axis gimbal)
- remove all the old unused code using gimbal control
This commit is contained in:
Gautier Hattenberger
2025-09-17 16:55:30 +02:00
committed by GitHub
parent 39ed0bb0b9
commit 528f9a5ade
18 changed files with 702 additions and 1490 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/joystick/attack3_joystick_raw.xml
+27
View File
@@ -0,0 +1,27 @@
<joystick>
<input>
<axis index="0" name="roll"/>
<axis index="1" name="pitch"/>
<axis index="2" name="throttle"/>
<button index="0" name="shoot"/>
<button index="1" name="down"/>
<button index="2" name="up"/>
<button index="3" name="left"/>
<button index="4" name="right"/>
<button index="5" name="button6"/>
<button index="6" name="button7"/>
<button index="7" name="button8"/>
<button index="8" name="button9"/>
<button index="9" name="button10"/>
<button index="10" name="button11"/>
</input>
<messages period="0.1">
<message class="datalink" name="JOYSTICK_RAW" send_always="false">
<field name="roll" value="Fit(roll, -126, 126, -9600, 9600)"/>
<field name="pitch" value="Fit(pitch, -126, 126, -9600, 9600)"/>
<field name="yaw" value="0"/>
<field name="throttle" value="Fit(-throttle, -126, 127, 0, 9600)"/>
</message>
</messages>
</joystick>
conf/modules/cam_gimbal.xml
+38
View File
@@ -0,0 +1,38 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cam_gimbal" dir="cam_control" task="control">
<doc>
<description>
Generic PAN/TILT camera gimbal control
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="cam control">
<dl_setting MAX="7" MIN="0" STEP="1" module="cam_control/cam_gimbal" VAR="cam_gimbal.mode" shortname="mode" values="OFF|JOYSTICK|ANGLES|NADIR|TARGET|WAYPOINT|AC_TARGET" handler="SetMode"/>
<dl_setting MAX="1" MIN="0" STEP="1" module="cam_control/cam_gimbal" VAR="cam_gimbal.lock" shortname="lock" values="UNLOCK|LOCK" handler="SetLock"/>
<dl_setting MAX="90" MIN="-90" STEP="1" var="cam_gimbal.tilt_angle" shortname="tilt" unit="rad" alt_unit="deg"/>
<dl_setting MAX="180" MIN="-180" STEP="1" var="cam_gimbal.pan_angle" shortname="pan" unit="rad" alt_unit="deg"/>
<dl_setting min="1" max="27" step="1" var="cam_gimbal.target_wp_id" shortname="wp id"/>
<dl_setting min="1" max="255" step="1" var="cam_gimbal.target_ac_id" shortname="ac id"/>
<dl_setting MAX="100." MIN="-100." STEP="1." var="cam_gimbal.target_pos.x" shortname="target x" unit="m"/>
<dl_setting MAX="100." MIN="-100." STEP="1." var="cam_gimbal.target_pos.y" shortname="target y" unit="m"/>
<dl_setting MAX="100." MIN="-100." STEP="1." var="cam_gimbal.target_pos.z" shortname="target z" unit="m"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<recommends>joystick</recommends>
</dep>
<header>
<file name="cam_gimbal.h"/>
</header>
<init fun="cam_gimbal_init()"/>
<periodic fun="cam_gimbal_periodic()" />
<makefile>
<file name="cam_gimbal.c"/>
<test firmware="fixedwing"/>
<test firmware="rotorcraft"/>
<test firmware="rover"/>
</makefile>
</module>
conf/modules/cam_point.xml
-56
View File
@@ -1,56 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cam_point" dir="cam_control">
<doc>
<description>Camera control for fixedwing</description>
</doc>
<settings name="complete">
<dl_settings NAME="control">
<dl_settings name="cam">
<dl_setting MAX="7" MIN="0" STEP="1" module="cam_control/cam" VAR="cam_mode" values="0FF|ANGLES|NADIR|XY_TARGET|WP_TARGET|AC_TARGET|STABILIZED|RC">
<strip_button name="AC_TARGET" value="5"/>
<strip_button name="WP_TARGET" value="4"/>
<strip_button name="XY_TARGET" value="3"/>
<strip_button name="NADIR" value="2"/>
<strip_button name="ANGLES" value="1"/>
<strip_button name="OFF" value="0"/>
</dl_setting>
</dl_settings>
<dl_settings name="angles">
<dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_tilt_c" unit="rad" alt_unit="deg"/>
<dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_pan_c" unit="rad" alt_unit="deg"/>
</dl_settings>
<dl_settings name="target">
<dl_setting min="1" max="27" step="1" module="cam_control/cam" var="cam_target_wp" shortname="wp"/>
</dl_settings>
</dl_settings>
</settings>
<settings name="pitch_only">
<dl_settings NAME="control">
<dl_settings name="cam">
<dl_setting MAX="1" MIN="0" STEP="1" module="cam_control/cam" VAR="cam_mode">
<strip_button name="ANGLES" value="1"/>
</dl_setting>
</dl_settings>
<dl_settings name="angles">
<dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_tilt_c" unit="rad" alt_unit="deg">
<strip_button name="Look Foreward" icon="lookfore.png" value="-0.5"/>
<strip_button name="Look Down" icon="lookdown.png" value="0.5"/>
</dl_setting>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="cam.h"/>
<file name="point.h"/>
</header>
<init fun="cam_init()"/>
<periodic fun="cam_periodic()" />
<makefile>
<define name="CAM"/>
<define name="MOBILE_CAM"/>
<define name="POINT_CAM"/>
<file name="cam.c"/>
<file name="point.c"/>
</makefile>
</module>
conf/modules/cam_roll.xml
-26
View File
@@ -1,26 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cam_roll" dir="cam_control">
<doc>
<description>Camera control on roll axis only</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="Cam">
<dl_setting MAX="45" MIN="-45" STEP="1" VAR="cam_roll_phi" module="cam_control/cam_roll" shortname="phi" unit="rad" alt_unit="deg" auto="true">
</dl_setting>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="cam_roll_mode" module="cam_control/cam_roll" shortname="manual - stablzd">
</dl_setting>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="cam.h"/>
</header>
<init fun="cam_init()"/>
<periodic fun="cam_periodic()" freq="10."/>
<makefile>
<define name="MOBILE_CAM"/>
<file name="cam_roll.c"/>
</makefile>
</module>
conf/modules/cam_segment.xml
-18
View File
@@ -1,18 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cam_segment" dir="cam_control">
<doc>
<description>Camera control to point a segment</description>
</doc>
<dep>
<depends>cam_point</depends>
</dep>
<header>
<file name="cam_segment.h"/>
</header>
<init fun="cam_segment_init()"/>
<periodic fun="cam_segment_periodic()" stop="cam_segment_stop()" freq="10." autorun="FALSE"/>
<makefile>
<file name="cam_segment.c"/>
</makefile>
</module>
conf/modules/gimbal_caddx_gm3.xml
+30
View File
@@ -0,0 +1,30 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="gimbal_caddx_gm3" dir="cam_control" task="control">
<doc>
<description>
Caddx gm3 gimbal control.
Can be used with actuators SBUS (1 wire) or PWM (multiple wires).
Define a servo named GIMBAL_CADDX_ROLL in airframe to activate roll control.
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="gimbal gm3">
<dl_setting MAX="60." MIN="-60." STEP="1." module="cam_control/gimbal_caddx_gm3" VAR="gimbal_caddx_gm3_roll" shortname="roll" unit="rad" alt_unit="deg"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>cam_gimbal</depends>
</dep>
<header>
<file name="gimbal_caddx_gm3.h"/>
</header>
<init fun="gimbal_caddx_gm3_init()"/>
<periodic fun="gimbal_caddx_gm3_periodic()" freq="10.0"/>
<makefile>
<file name="gimbal_caddx_gm3.c"/>
<test/>
</makefile>
</module>
sw/airborne/modules/cam_control/cam.c
-318
View File
@@ -1,318 +0,0 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam.c
* \brief Pan/Tilt camera library
*
*/
#include <math.h>
#include "cam.h"
#include "modules/nav/common_nav.h" //needed for WaypointX, WaypointY and ground_alt
#include "autopilot.h"
#include "generated/flight_plan.h"
#include "state.h"
#ifdef POINT_CAM
#include "point.h"
#endif // POINT_CAM
#include "modules/datalink/telemetry.h"
#ifdef TEST_CAM
float test_cam_estimator_x;
float test_cam_estimator_y;
float test_cam_estimator_z;
float test_cam_estimator_phi;
float test_cam_estimator_theta;
float test_cam_estimator_hspeed_dir;
#endif // TEST_CAM
//FIXME: use radians
#ifdef CAM_PAN_NEUTRAL
#if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
#error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
#endif
#if (CAM_PAN_NEUTRAL == CAM_PAN_MIN)
#error CAM_PAN_MIN has to be different from CAM_PAN_NEUTRAL
#endif
#endif
//FIXME: use radians
#ifdef CAM_TILT_NEUTRAL
#if ((CAM_TILT_MAX) == (CAM_TILT_NEUTRAL))
#error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
#endif
#if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
#error CAM_TILT_MIN has to be different from CAM_TILT_NEUTRAL
#endif
#endif
//FIXME: use radians
#ifndef CAM_PAN0
#define CAM_PAN0 RadOfDeg(0)
#endif
float cam_pan_c;
//FIXME: use radians
#ifndef CAM_TILT0
#define CAM_TILT0 RadOfDeg(0)
#endif
float cam_tilt_c;
float cam_phi_c;
float cam_theta_c;
float cam_target_x, cam_target_y, cam_target_alt;
uint8_t cam_target_wp;
uint8_t cam_target_ac;
#ifndef CAM_MODE0
#define CAM_MODE0 CAM_MODE_OFF
#endif
uint8_t cam_mode;
bool cam_lock;
int16_t cam_pan_command;
int16_t cam_tilt_command;
void cam_nadir(void);
void cam_angles(void);
void cam_target(void);
void cam_waypoint_target(void);
void cam_ac_target(void);
static void send_cam(struct transport_tx *trans, struct link_device *dev)
{
int16_t x = cam_target_x;
int16_t y = cam_target_y;
int16_t phi = DegOfRad(cam_phi_c);
int16_t theta = DegOfRad(cam_theta_c);
pprz_msg_send_CAM(trans, dev, AC_ID, &phi, &theta, &x, &y);
}
#ifdef SHOW_CAM_COORDINATES
static void send_cam_point(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_CAM_POINT(trans, dev, AC_ID,
&cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
}
#endif
void cam_init(void)
{
cam_mode = CAM_MODE0;
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM, send_cam);
#ifdef SHOW_CAM_COORDINATES
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM_POINT, send_cam_point);
#endif
}
void cam_periodic(void)
{
#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
//Position the camera for straight view.
if (autopilot_get_mode() == AP_MODE_AUTO2) {
#endif
switch (cam_mode) {
case CAM_MODE_OFF:
cam_pan_c = RadOfDeg(CAM_PAN0);
cam_tilt_c = RadOfDeg(CAM_TILT0);
cam_angles();
break;
case CAM_MODE_ANGLES:
cam_angles();
break;
case CAM_MODE_NADIR:
cam_nadir();
break;
case CAM_MODE_XY_TARGET:
cam_target();
break;
case CAM_MODE_WP_TARGET:
cam_waypoint_target();
break;
case CAM_MODE_AC_TARGET:
cam_ac_target();
break;
// In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
// The "TARGET" waypoint coordinates are not used.
// If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
case CAM_MODE_STABILIZED:
cam_waypoint_target();
break;
// In this mode the angles come from the pan and tilt radio channels.
// The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
// in order to calculate the coordinates of where the camera is looking.
case CAM_MODE_RC:
cam_waypoint_target();
break;
default:
break;
}
#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
} else if (autopilot_get_mode() == AP_MODE_AUTO1) {
//Position the camera for straight view.
#if defined(CAM_TILT_POSITION_FOR_FPV)
cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
#else
cam_tilt_c = RadOfDeg(90);
#endif
#if defined(CAM_PAN_POSITION_FOR_FPV)
cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
#else
cam_pan_c = RadOfDeg(0);
#endif
cam_angles();
#ifdef SHOW_CAM_COORDINATES
cam_point_lon = 0;
cam_point_lat = 0;
cam_point_distance_from_home = 0;
#endif
}
#endif
#if defined(COMMAND_CAM_PWR_SW)
if (video_tx_state) { command_set(COMMAND_CAM_PWR_SW, MAX_PPRZ); } else { command_set(COMMAND_CAM_PWR_SW, MIN_PPRZ); }
#elif defined(VIDEO_TX_SWITCH)
if (video_tx_state) { LED_OFF(VIDEO_TX_SWITCH); } else { LED_ON(VIDEO_TX_SWITCH); }
#endif
}
/** Computes the servo values from cam_pan_c and cam_tilt_c */
void cam_angles(void)
{
float cam_pan = 0;
float cam_tilt = 0;
if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
cam_pan_c = RadOfDeg(CAM_PAN_MAX);
} else {
if (cam_pan_c < RadOfDeg(CAM_PAN_MIN)) {
cam_pan_c = RadOfDeg(CAM_PAN_MIN);
}
}
if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)) {
cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
} else {
if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN)) {
cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
}
}
#ifdef CAM_PAN_NEUTRAL
float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
if (pan_diff > 0) {
cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
} else {
cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
}
#else
cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN));
#endif
#ifdef CAM_TILT_NEUTRAL
float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
if (tilt_diff > 0) {
cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
} else {
cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
}
#else
cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(
CAM_TILT_MAX - CAM_TILT_MIN));
#endif
cam_pan = TRIM_PPRZ(cam_pan);
cam_tilt = TRIM_PPRZ(cam_tilt);
cam_phi_c = cam_pan_c;
cam_theta_c = cam_tilt_c;
#ifdef COMMAND_CAM_PAN
command_set(COMMAND_CAM_PAN, cam_pan);
#endif
#ifdef COMMAND_CAM_TILT
command_set(COMMAND_CAM_TILT, cam_tilt);
#endif
}
/** Computes the right angles from target_x, target_y, target_alt */
void cam_target(void)
{
#ifdef TEST_CAM
vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#else
struct EnuCoor_f *pos = stateGetPositionEnu_f();
struct FloatEulers *att = stateGetNedToBodyEulers_f();
vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt,
att->phi, att->theta, stateGetHorizontalSpeedDir_f(),
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#endif
cam_angles();
}
/** Point straight down */
void cam_nadir(void)
{
struct EnuCoor_f *pos = stateGetPositionEnu_f();
#ifdef TEST_CAM
cam_target_x = test_cam_estimator_x;
cam_target_y = test_cam_estimator_y;
#else
cam_target_x = pos->x;
cam_target_y = pos->y;
#endif
cam_target_alt = -10;
cam_target();
}
void cam_waypoint_target(void)
{
if (cam_target_wp < nb_waypoint) {
cam_target_x = WaypointX(cam_target_wp);
cam_target_y = WaypointY(cam_target_wp);
}
cam_target_alt = ground_alt;
cam_target();
}
#ifdef TRAFFIC_INFO
#include "modules/multi/traffic_info.h"
void cam_ac_target(void)
{
struct EnuCoor_f ac_pos *ac = acInfoGetPositionEnu_f(cam_target_ac);
cam_target_x = ac->x;
cam_target_y = ac->y;
cam_target_alt = acInfoGetPositionUtm_f()->alt;
cam_target();
}
#else
void cam_ac_target(void) {}
#endif // TRAFFIC_INFO
sw/airborne/modules/cam_control/cam.h
-99
View File
@@ -1,99 +0,0 @@
/*
* Copyright (C) 2005- Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam.h
* \brief Pan/Tilt camera API
*
*/
#ifndef CAM_H
#define CAM_H
#include <inttypes.h>
#include "modules/core/commands.h"
#define CAM_MODE_OFF 0 /* Do nothing */
#define CAM_MODE_ANGLES 1 /* Input: servo angles */
#define CAM_MODE_NADIR 2 /* Input: () */
#define CAM_MODE_XY_TARGET 3 /* Input: target_x, target_y */
#define CAM_MODE_WP_TARGET 4 /* Input: waypoint no */
#define CAM_MODE_AC_TARGET 5 /* Input: ac id */
#define CAM_MODE_STABILIZED 6 // Stabilized mode, input: camera angles from the pan and tilt radio channels, output pointing coordinates.
#define CAM_MODE_RC 7 // Manual mode, input: camera angles from the pan and tilt radio channels, output servo positions.
//FIXME: use radians
#ifndef CAM_PAN_MAX
#define CAM_PAN_MAX 90
#endif
#ifndef CAM_PAN_MIN
#define CAM_PAN_MIN -90
#endif
#ifndef CAM_TILT_MAX
#define CAM_TILT_MAX 90
#endif
#ifndef CAM_TILT_MIN
#define CAM_TILT_MIN -90
#endif
extern uint8_t cam_mode;
extern uint8_t cam_lock;
extern float cam_phi_c, cam_theta_c;
extern float cam_pan_c, cam_tilt_c;
/* pan (move left and right), tilt (move up and down) */
/** Radians, for CAM_MODE_ANGLES mode */
extern float cam_target_x, cam_target_y, cam_target_alt;
/** For CAM_MODE_XY_TARGET mode */
extern uint8_t cam_target_wp;
/** For CAM_MODE_WP_TARGET mode */
extern uint8_t cam_target_ac;
/** For CAM_MODE_AC_TARGET mode */
void cam_periodic(void);
void cam_init(void);
extern int16_t cam_pan_command;
#define cam_SetPanCommand(x) { cam_pan_command = x; command_set(COMMAND_CAM_PAN, cam_pan_command);}
extern int16_t cam_tilt_command;
#define cam_SetTiltCommand(x) { cam_tilt_command = x; command_set(COMMAND_CAM_TILT, cam_tilt_command);}
#ifdef TEST_CAM
extern float test_cam_estimator_x;
extern float test_cam_estimator_y;
extern float test_cam_estimator_z;
extern float test_cam_estimator_phi;
extern float test_cam_estimator_theta;
extern float test_cam_estimator_hspeed_dir;
#endif // TEST_CAM
#if defined(COMMAND_CAM_PWR_SW) || defined(VIDEO_TX_SWITCH)
extern bool video_tx_state;
#define VIDEO_TX_ON() { video_tx_state = 1; 0; }
#define VIDEO_TX_OFF() { video_tx_state = 0; 0; }
#endif
#endif // CAM_H
sw/airborne/modules/cam_control/cam_gimbal.c
+367
View File
@@ -0,0 +1,367 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* 2025 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam_gimbal.c
* \brief Pan/Tilt camera gimbal control
*
*/
#include "cam_gimbal.h"
#include "autopilot.h"
#if FIXEDWING_FIRMWARE
#include "modules/nav/common_nav.h"
#else
#include "modules/nav/waypoints.h"
#endif
#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"
// Default idle command
#ifndef CAM_GIMBAL_PAN0
#define CAM_GIMBAL_PAN0 0
#endif
#ifndef CAM_GIMBAL_TILT0
#define CAM_GIMBAL_TILT0 0
#endif
// Minimum and maximum angles
// used to convert angles to commands, assuming a linear interpolation
#ifndef CAM_GIMBAL_PAN_MAX
#define CAM_GIMBAL_PAN_MAX RadOfDeg(90.f)
#endif
#ifndef CAM_GIMBAL_PAN_MIN
#define CAM_GIMBAL_PAN_MIN -CAM_GIMBAL_PAN_MAX
#endif
#ifndef CAM_GIMBAL_TILT_MAX
#define CAM_GIMBAL_TILT_MAX RadOfDeg(90.f)
#endif
#ifndef CAM_GIMBAL_TILT_MIN
#define CAM_GIMBAL_TILT_MIN -CAM_GIMBAL_TILT_MAX
#endif
// Default position and orientation of the gimbal in body frame
#ifndef CAM_GIMBAL_POS_X
#define CAM_GIMBAL_POS_X 0.f
#endif
#ifndef CAM_GIMBAL_POS_Y
#define CAM_GIMBAL_POS_Y 0.f
#endif
#ifndef CAM_GIMBAL_POS_Z
#define CAM_GIMBAL_POS_Z 0.f
#endif
#ifndef CAM_GIMBAL_TO_BODY_PHI
#define CAM_GIMBAL_TO_BODY_PHI 0.f
#endif
#ifndef CAM_GIMBAL_TO_BODY_THETA
#define CAM_GIMBAL_TO_BODY_THETA 0.f
#endif
#ifndef CAM_GIMBAL_TO_BODY_PSI
#define CAM_GIMBAL_TO_BODY_PSI 0.f
#endif
// Global cam structure
struct CamGimbal cam_gimbal;
// ABI message bind
static abi_event joystick_ev;
static void joystick_cb(uint8_t sender_id UNUSED, int16_t roll, int16_t pitch, int16_t yaw UNUSED, int16_t throttle UNUSED)
{
cam_gimbal.pan_joystick = roll;
cam_gimbal.tilt_joystick = pitch;
}
static void send_cam(struct transport_tx *trans, struct link_device *dev)
{
int16_t x = cam_gimbal.target_pos.x;
int16_t y = cam_gimbal.target_pos.y;
int16_t phi = DegOfRad(cam_gimbal.pan_angle);
int16_t theta = DegOfRad(cam_gimbal.tilt_angle);
pprz_msg_send_CAM(trans, dev, AC_ID, &phi, &theta, &x, &y);
}
#if CAM_SHOW_COORDINATES
static void send_cam_point(struct transport_tx *trans, struct link_device *dev)
{
struct LlaCoor_f target_lla;
struct EcefCoor_f target_ecef;
ecef_of_enu_point_f(&target_ecef, stateGetNedOrigin_f(), cam_gimbal.target_pos);
lla_of_ecef_f(&target_lla, &target_ecef);
uint16_t dist_from_home = 0;
pprz_msg_send_CAM_POINT(trans, dev, AC_ID, &dist_from_home, &target_lla.lat, &target_lla.lon);
}
#endif
/** 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
*/
static void default_compute_angles(struct FloatVect3 dir, float *pan, float *tilt)
{
*pan = atan2f(dir.y, dir.x);
*tilt = asinf(-dir.z);
}
/** Computes the servo values from pan and tilt angles */
static void cam_gimbal_angles(struct CamGimbal *cam)
{
Bound(cam->pan_angle, cam->pan_min, cam->pan_max);
Bound(cam->tilt_angle, cam->tilt_min, cam->tilt_max);
if (!cam->lock) {
float delta_pan = cam->pan_max - cam->pan_min;
float delta_tilt = cam->tilt_max - cam->tilt_min;
cam->pan_cmd = (int16_t) MAX_PPRZ * ((2.f / delta_pan) * (cam->pan_angle - cam->pan_min) - 1.f);
cam->tilt_cmd = (int16_t) MAX_PPRZ * ((2.f / delta_tilt) * (cam->tilt_angle - cam->tilt_min) - 1.f);
}
}
/** Computes the right angles from target position */
static void cam_gimbal_target(struct CamGimbal *cam)
{
struct FloatRMat *ltp_to_body = stateGetNedToBodyRMat_f();
struct NedCoor_f pos = *stateGetPositionNed_f();
struct NedCoor_f target;
ENU_OF_TO_NED(target, cam->target_pos);
// compute looking direction in gimbal frame
// o: Earth frame (ltp)
// b: body frame
// g: gimbal frame
// D/o = normalized(Pt/o - Pg/o = Pt/o - (Pb/o + Pg/b))
struct FloatVect3 dir_ltp;
VECT3_DIFF(dir_ltp, target, pos);
VECT3_SUB(dir_ltp, cam->gimbal_pos);
float_vect3_normalize(&dir_ltp);
// rotate D/o to get D/g = Rg/o D/o = inv(Rb/g) Rb/o D/o
struct FloatVect3 dir_body;
float_rmat_vmult(&dir_body, ltp_to_body, &dir_ltp);
struct FloatVect3 dir_gimbal;
float_rmat_transp_vmult(&dir_gimbal, &cam->gimbal_to_body, &dir_body);
// compute angles from direction
cam->compute_angles(dir_gimbal, &cam->pan_angle, &cam->tilt_angle);
// apply angles
cam_gimbal_angles(cam);
}
/** Point straight down */
static void cam_gimbal_nadir(struct CamGimbal *cam)
{
struct EnuCoor_f target = *stateGetPositionEnu_f();
target.z -= 10.f; // force looking below current position
cam_gimbal_set_target_pos(cam, target);
cam_gimbal_target(cam);
}
static void cam_gimbal_waypoint_target(struct CamGimbal *cam)
{
if (cam->target_wp_id < nb_waypoint) {
struct EnuCoor_f target;
target.x = WaypointX(cam->target_wp_id);
target.y = WaypointY(cam->target_wp_id);
target.z = Min(0.f, stateGetPositionEnu_f()->z); // ground alt or A/C alt if lower
cam_gimbal_set_target_pos(cam, target);
cam_gimbal_target(cam);
}
}
static void cam_gimbal_ac_target(struct CamGimbal *cam UNUSED)
{
#ifdef TRAFFIC_INFO
struct EnuCoor_f target = *acInfoGetPositionEnu_f(cam.target_ac_id);
cam_gimbal_target(cam);
#endif
}
static void cam_gimbal_joystick(struct CamGimbal *cam UNUSED)
{
cam_gimbal_set_pan_command(cam, cam->pan_joystick);
cam_gimbal_set_tilt_command(cam, cam->tilt_joystick);
}
void cam_gimbal_setup_angles(struct CamGimbal *cam,
float pan_max, float pan_min,
float tilt_max, float tilt_min)
{
cam->pan_max = pan_max;
cam->pan_min = pan_min;
cam->tilt_max = tilt_max;
cam->tilt_min = tilt_min;
}
void cam_gimbal_setup_mounting(struct CamGimbal *cam,
struct FloatEulers gimbal_to_body_eulers,
struct FloatVect3 gimbal_pos)
{
float_rmat_of_eulers(&cam->gimbal_to_body, &gimbal_to_body_eulers);
cam->gimbal_pos = gimbal_pos;
}
void cam_gimbal_set_angles_callback(struct CamGimbal *cam, cam_angles_from_dir compute_angles)
{
cam->compute_angles = compute_angles;
}
void cam_gimbal_set_mode(struct CamGimbal *cam, uint8_t mode)
{
if (mode < CAM_GIMBAL_MODE_NB) {
cam->mode = mode;
} else {
cam->mode = CAM_GIMBAL_MODE_OFF;
}
}
void cam_gimbal_set_lock(struct CamGimbal *cam, bool lock)
{
cam->lock = lock;
}
void cam_gimbal_set_pan_command(struct CamGimbal *cam, int16_t pan)
{
cam->pan_cmd = TRIM_PPRZ(pan);
}
void cam_gimbal_set_tilt_command(struct CamGimbal *cam, int16_t tilt)
{
cam->tilt_cmd = TRIM_PPRZ(tilt);
}
void cam_gimbal_set_angles_rad(struct CamGimbal *cam, float pan, float tilt)
{
cam->pan_angle = pan;
cam->tilt_angle = tilt;
Bound(cam->pan_angle, cam->pan_min, cam->pan_max);
Bound(cam->tilt_angle, cam->tilt_min, cam->tilt_max);
}
void cam_gimbal_set_angles_deg(struct CamGimbal *cam, float pan, float tilt)
{
cam->pan_angle = RadOfDeg(pan);
cam->tilt_angle = RadOfDeg(tilt);
Bound(cam->pan_angle, cam->pan_min, cam->pan_max);
Bound(cam->tilt_angle, cam->tilt_min, cam->tilt_max);
}
void cam_gimbal_set_target_pos(struct CamGimbal *cam, struct EnuCoor_f target)
{
cam->target_pos = target;
}
void cam_gimbal_set_wp_id(struct CamGimbal *cam, uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
cam->target_wp_id = wp_id;
}
}
void cam_gimbal_set_ac_id(struct CamGimbal *cam, uint8_t ac_id)
{
cam->target_ac_id = ac_id;
}
/** Run camera control
*/
void cam_gimbal_run(struct CamGimbal *cam)
{
switch (cam->mode) {
case CAM_GIMBAL_MODE_OFF:
cam_gimbal_set_pan_command(cam, CAM_GIMBAL_PAN0);
cam_gimbal_set_pan_command(cam, CAM_GIMBAL_TILT0);
break;
case CAM_GIMBAL_MODE_JOYSTICK:
cam_gimbal_joystick(cam);
break;
case CAM_GIMBAL_MODE_ANGLES:
cam_gimbal_angles(cam);
break;
case CAM_GIMBAL_MODE_NADIR:
cam_gimbal_nadir(cam);
break;
case CAM_GIMBAL_MODE_TARGET:
cam_gimbal_target(cam);
break;
case CAM_GIMBAL_MODE_WAYPOINT:
cam_gimbal_waypoint_target(cam);
break;
case CAM_GIMBAL_MODE_AC_TARGET:
cam_gimbal_ac_target(cam);
break;
default:
break;
}
}
/** Init module
*/
void cam_gimbal_init(void)
{
// apply default settings
struct FloatEulers gimbal_to_body = {
CAM_GIMBAL_TO_BODY_PHI,
CAM_GIMBAL_TO_BODY_THETA,
CAM_GIMBAL_TO_BODY_PSI
};
struct FloatVect3 gimbal_pos = {
CAM_GIMBAL_POS_X,
CAM_GIMBAL_POS_Y,
CAM_GIMBAL_POS_Z
};
cam_gimbal_setup_angles(&cam_gimbal,
CAM_GIMBAL_PAN_MAX, CAM_GIMBAL_PAN_MIN,
CAM_GIMBAL_TILT_MAX, CAM_GIMBAL_TILT_MIN);
cam_gimbal_setup_mounting(&cam_gimbal, gimbal_to_body, gimbal_pos);
cam_gimbal_set_angles_callback(&cam_gimbal, default_compute_angles);
AbiBindMsgJOYSTICK(ABI_BROADCAST, &joystick_ev, joystick_cb);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM, send_cam);
#ifdef CAM_SHOW_COORDINATES
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM_POINT, send_cam_point);
#endif
}
/** Periodic call (run control)
*/
void cam_gimbal_periodic(void)
{
cam_gimbal_run(&cam_gimbal);
// update command if possible
#ifdef COMMAND_CAM_PAN
command_set(COMMAND_CAM_PAN, cam_gimbal.pan_cmd);
#endif
#ifdef COMMAND_CAM_TILT
command_set(COMMAND_CAM_TILT, cam_gimbal.tilt_cmd);
#endif
}
sw/airborne/modules/cam_control/cam_gimbal.h
+111
View File
@@ -0,0 +1,111 @@
/*
* Copyright (C) 2005 Pascal Brisset, Antoine Drouin
* 2025 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam_gimbal.h
* \brief Pan/Tilt camera gimbal control
*
*/
#ifndef CAM_GIMBAL_H
#define CAM_GIMBAL_H
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#define CAM_GIMBAL_MODE_OFF 0 // Do nothing
#define CAM_GIMBAL_MODE_JOYSTICK 1 // Manual mode from ABI JOYSTICK message
#define CAM_GIMBAL_MODE_ANGLES 2 // Input: servo angles
#define CAM_GIMBAL_MODE_NADIR 3 // Input: look down
#define CAM_GIMBAL_MODE_TARGET 4 // Input: target position
#define CAM_GIMBAL_MODE_WAYPOINT 5 // Input: waypoint no
#define CAM_GIMBAL_MODE_AC_TARGET 6 // Input: ac id
#define CAM_GIMBAL_MODE_NB 7 // number of modes
/** Function pointer to return cam angle from a specified direction
*
* The direction is the unit vector from the camera position to the target
* expressed in the gimbal frame.
* The resulting angles depends on the type of gimbal that is used,
* in particular the number and order of rotations.
* This function is provided by the user as it is specific to each mounting.
* It returns the pan and tilt angles.
*/
typedef void (*cam_angles_from_dir)(struct FloatVect3 dir, float *pan, float *tilt);
struct CamGimbal {
uint8_t mode; ///< gimbal control mode
bool lock; ///< lock current command
int16_t pan_cmd; ///< pan command [pprz]
int16_t tilt_cmd; ///< tilt command [pprz]
float pan_max; ///< pan angle at maximum command
float pan_min; ///< pan angle at minimum command
float tilt_max; ///< tilt angle at maximum command
float tilt_min; ///< tilt angle at minimum command
struct FloatRMat gimbal_to_body; ///< rotation matrix from gimbal to body frame
struct FloatVect3 gimbal_pos; ///< position of the gimbal in body NED frame [m]
cam_angles_from_dir compute_angles; ///< cam angles from looking direction callback
float pan_angle; ///< pan angle [rad]
float tilt_angle; ///< tilt angle [rad]
struct EnuCoor_f target_pos; ///< target point in ENU world frame [m]
uint8_t target_wp_id; ///< waypoint ID to track
uint8_t target_ac_id; ///< aircraft ID to track
int16_t pan_joystick; ///< pan command from joystick
int16_t tilt_joystick; ///< tilt command from joystick
};
extern struct CamGimbal cam_gimbal;
extern void cam_gimbal_init(void);
extern void cam_gimbal_periodic(void);
// API for internal and external use
extern void cam_gimbal_setup_angles(struct CamGimbal *cam,
float pan_max, float pan_min,
float tilt_max, float tilt_min);
extern void cam_gimbal_setup_mounting(struct CamGimbal *cam,
struct FloatEulers gimbal_to_body,
struct FloatVect3 gimbal_pos);
extern void cam_gimbal_set_angles_callback(struct CamGimbal *cam, cam_angles_from_dir compute_angles);
extern void cam_gimbal_run(struct CamGimbal *cam);
extern void cam_gimbal_set_mode(struct CamGimbal *cam, uint8_t mode);
extern void cam_gimbal_set_lock(struct CamGimbal *cam, bool lock);
extern void cam_gimbal_set_pan_command(struct CamGimbal *cam, int16_t pan);
extern void cam_gimbal_set_tilt_command(struct CamGimbal *cam, int16_t tilt);
extern void cam_gimbal_set_angles_rad(struct CamGimbal *cam, float pan, float tilt);
extern void cam_gimbal_set_angles_deg(struct CamGimbal *cam, float pan, float tilt);
extern void cam_gimbal_set_target_pos(struct CamGimbal *cam, struct EnuCoor_f target);
extern void cam_gimbal_set_wp_id(struct CamGimbal *cam, uint8_t wp_id);
extern void cam_gimbal_set_ac_id(struct CamGimbal *cam, uint8_t ac_id);
// settings handler
#define cam_gimbal_SetMode(x) cam_gimbal_set_mode(&cam_gimbal,x)
#define cam_gimbal_SetLock(x) cam_gimbal_set_lock(&cam_gimbal, x)
#define cam_gimbal_SetPanCommand(x) cam_gimbal_set_pan_command(&cam_gimbal, x)
#define cam_gimbal_SetTiltCommand(x) cam_gimbal_set_pan_command(&cam_gimbal, x)
#endif // CAM_GIMBAL_H
sw/airborne/modules/cam_control/cam_roll.c
-77
View File
@@ -1,77 +0,0 @@
/*
*
* Copyright (C) 2003-2011 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam.c
* \brief Pan/Tilt camera library
*
*/
#include <math.h>
#include "cam.h"
#include "firmwares/fixedwing/nav.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
#include "state.h"
#include "modules/core/commmands.h"
#ifndef CAM_PHI_MAX
#define CAM_PHI_MAX RadOfDeg(45)
#endif
float cam_roll_phi; /* radian */
float phi_c; /* radian */
float theta_c; /* have to be defined for telemetry message */
float target_x, target_y, target_alt;
#ifdef MOBILE_CAM
#define MODE_MANUAL 0
#define MODE_STABILIZED 1
#ifndef CAM_ROLL_START_MODE
#define CAM_ROLL_START_MODE MODE_MANUAL
#endif
uint8_t cam_roll_mode;
void cam_init(void)
{
cam_roll_mode = CAM_ROLL_START_MODE;
}
void cam_periodic(void)
{
switch (cam_roll_mode) {
case MODE_STABILIZED:
phi_c = cam_roll_phi + stateGetNedToBodyEulers_f()->phi;
break;
case MODE_MANUAL:
phi_c = cam_roll_phi;
break;
default:
phi_c = 0;
}
command_set(COMMAND_CAM_ROLL, TRIM_PPRZ(phi_c * MAX_PPRZ / CAM_PHI_MAX));
}
#endif // MOBILE_CAM
sw/airborne/modules/cam_control/cam_roll.h
-29
View File
@@ -1,29 +0,0 @@
/*
* Copyright (C) 2003-2011 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef CAM_ROLL_H
#define CAM_ROLL_H
extern uint8_t cam_roll_mode;
extern float cam_roll_phi;
#endif /* CAM_ROLL_H */
sw/airborne/modules/cam_control/cam_segment.c
-48
View File
@@ -1,48 +0,0 @@
/*
* Copyright (C) 2011 Gautier Hattenberger
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam_segment.c
* \brief camera control to track a segment using the general cam driver (target mode)
*
* initial version: pointing towards the carrot
*/
#include "modules/cam_control/cam_segment.h"
#include "modules/cam_control/cam.h"
#include "firmwares/fixedwing/nav.h"
void cam_segment_init(void)
{
}
void cam_segment_stop(void)
{
cam_mode = CAM_MODE_OFF;
}
void cam_segment_periodic(void)
{
cam_mode = CAM_MODE_XY_TARGET;
cam_target_x = desired_x;
cam_target_y = desired_y;
cam_target_alt = ground_alt;
}
sw/airborne/modules/cam_control/cam_segment.h
-35
View File
@@ -1,35 +0,0 @@
/*
* Copyright (C) 2011 Gautier Hattenberger
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file cam_segment.c
* \brief camera control to track a segment using the general cam driver (target mode)
*
* initial version: pointing towards the carrot
*/
#ifndef CAM_SEGMENT_H
#define CAM_SEGMENT_H
extern void cam_segment_init(void);
extern void cam_segment_stop(void);
extern void cam_segment_periodic(void);
#endif
sw/airborne/modules/cam_control/gimbal_caddx_gm3.c
+91
View File
@@ -0,0 +1,91 @@
/*
* Copyright (C) 2025 Julia Cabarbaye <julia.cabarbaye1@gmail.com>
* 2025 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file "modules/cam_control/gimbal_caddx_gm3.c"
* @author Julia Cabarbaye <julia.cabarbaye1@gmail.com>
* caddx gm3 gimbal control sbus
*/
#include "modules/cam_control/gimbal_caddx_gm3.h"
#include "modules/cam_control/cam_gimbal.h"
#include "modules/actuators/actuators.h"
#include "generated/airframe.h"
#include "modules/datalink/datalink.h"
#include "pprzlink/dl_protocol.h" // datalink messages
// mechanical characteristics
#define GIMBAL_CADDX_PAN_MAX RadOfDeg(160.f)
#define GIMBAL_CADDX_TILT_MAX RadOfDeg(120.f)
#define GIMBAL_CADDX_ROLL_MAX RadOfDeg(60.f)
#define GIMBAL_CADDX_TILT_OFFSET RadOfDeg(15.f)
float gimbal_caddx_gm3_roll;
/** Compute pan and tilt angle for the 3-axis gimbal CaddX GM3
*
* The rotations from gimbal to camera frame are: pan (z), roll (x), tilt (y)
* In addition, an extra mechanical offset exists along the tilt axis between the pan and roll
* rotations. Considering the wide angle camera and the pain to inverse the resulting equation,
* this angle is neglected.
* Therefore, we have:
* -> sin(tilt) = - uz / cos(roll)
* -> tan(pan) = (uy / ux) - sin(roll)*tan(tilt)
*/
static void gimbal_caddx_compute_angles(struct FloatVect3 dir, float *pan, float *tilt)
{
float roll = gimbal_caddx_gm3_roll;
BoundAbs(roll, GIMBAL_CADDX_ROLL_MAX);
*tilt = asinf(- dir.z / cosf(roll));
*pan = atan2f(dir.y - sinf(roll)*tanf(*tilt)*dir.x, dir.x);
}
void gimbal_caddx_gm3_init(void)
{
// Set the cam control with the specific parameter of CaddX GM3 gimble
cam_gimbal_setup_angles(&cam_gimbal,
GIMBAL_CADDX_PAN_MAX, -GIMBAL_CADDX_PAN_MAX,
GIMBAL_CADDX_TILT_MAX, -GIMBAL_CADDX_TILT_MAX);
cam_gimbal_set_angles_callback(&cam_gimbal, gimbal_caddx_compute_angles);
// Set mode
#ifdef SERVO_GIMBAL_CADDX_MODE_NEUTRAL
ActuatorSet(GIMBAL_CADDX_MODE, MIN_PPRZ);
#endif
#ifdef SERVO_GIMBAL_CADDX_SENS_NEUTRAL
ActuatorSet(GIMBAL_CADDX_SENS, 0);
#endif
gimbal_caddx_gm3_roll = 0.f;
#ifdef SERVO_GIMBAL_CADDX_ROLL_NEUTRAL
// GM2 model can be used as a GM3 without roll
ActuatorSet(GIMBAL_CADDX_ROLL, 0);
#endif
}
void gimbal_caddx_gm3_periodic(void)
{
#ifdef SERVO_GIMBAL_CADDX_ROLL_NEUTRAL
float roll_cmd = gimbal_caddx_gm3_roll * MAX_PPRZ / GIMBAL_CADDX_ROLL_MAX;
ActuatorSet(GIMBAL_CADDX_ROLL, roll_cmd);
#endif
}
sw/airborne/modules/cam_control/gimbal_caddx_gm3.h
+38
View File
@@ -0,0 +1,38 @@
/*
* Copyright (C) 2025 Julia Cabarbaye <julia.cabarbaye1@gmail.com>
* Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file "modules/cam_control/gimbal_caddx_gm3.h"
* @author Julia Cabarbaye <julia.cabarbaye1@gmail.com>
* caddx gm3 gimbal control sbus
*/
#ifndef GIMBAL_CADDX_GM3_H
#define GIMBAL_CADDX_GM3_H
#include "std.h"
extern float gimbal_caddx_gm3_roll;
extern void gimbal_caddx_gm3_init(void);
extern void gimbal_caddx_gm3_periodic(void);
#endif // GIMBAL_CADDX_GM3_H
sw/airborne/modules/cam_control/point.c
-746
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/cam_control/point.h
-38
View File
@@ -1,38 +0,0 @@
/*
* Copyright (C) 2005-2008 Arnold Schroeter
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef POINT_H
#define POINT_H
#if defined(SHOW_CAM_COORDINATES)
extern uint16_t cam_point_distance_from_home;
extern float cam_point_lon, cam_point_lat;
extern float distance_correction;
#endif
void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
float fRollAngle, float fPitchAngle, float fYawAngle,
float fObjectEast, float fObjectNorth, float fAltitude,
float *fPan, float *fTilt);
#endif /* POINT_H */