From 528f9a5adef428669351c8e0d21f6358c49f732f Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 17 Sep 2025 16:55:30 +0200
Subject: [PATCH] [cam_gimbal] rewrite the pan/tilt control module (#3538)

- 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
---
 conf/joystick/attack3_joystick_raw.xml        |  27 +
 conf/modules/cam_gimbal.xml                   |  38 +
 conf/modules/cam_point.xml                    |  56 --
 conf/modules/cam_roll.xml                     |  26 -
 conf/modules/cam_segment.xml                  |  18 -
 conf/modules/gimbal_caddx_gm3.xml             |  30 +
 sw/airborne/modules/cam_control/cam.c         | 318 --------
 sw/airborne/modules/cam_control/cam.h         |  99 ---
 sw/airborne/modules/cam_control/cam_gimbal.c  | 367 +++++++++
 sw/airborne/modules/cam_control/cam_gimbal.h  | 111 +++
 sw/airborne/modules/cam_control/cam_roll.c    |  77 --
 sw/airborne/modules/cam_control/cam_roll.h    |  29 -
 sw/airborne/modules/cam_control/cam_segment.c |  48 --
 sw/airborne/modules/cam_control/cam_segment.h |  35 -
 .../modules/cam_control/gimbal_caddx_gm3.c    |  91 +++
 .../modules/cam_control/gimbal_caddx_gm3.h    |  38 +
 sw/airborne/modules/cam_control/point.c       | 746 ------------------
 sw/airborne/modules/cam_control/point.h       |  38 -
 18 files changed, 702 insertions(+), 1490 deletions(-)
 create mode 100644 conf/joystick/attack3_joystick_raw.xml
 create mode 100644 conf/modules/cam_gimbal.xml
 delete mode 100644 conf/modules/cam_point.xml
 delete mode 100644 conf/modules/cam_roll.xml
 delete mode 100644 conf/modules/cam_segment.xml
 create mode 100644 conf/modules/gimbal_caddx_gm3.xml
 delete mode 100644 sw/airborne/modules/cam_control/cam.c
 delete mode 100644 sw/airborne/modules/cam_control/cam.h
 create mode 100644 sw/airborne/modules/cam_control/cam_gimbal.c
 create mode 100644 sw/airborne/modules/cam_control/cam_gimbal.h
 delete mode 100644 sw/airborne/modules/cam_control/cam_roll.c
 delete mode 100644 sw/airborne/modules/cam_control/cam_roll.h
 delete mode 100644 sw/airborne/modules/cam_control/cam_segment.c
 delete mode 100644 sw/airborne/modules/cam_control/cam_segment.h
 create mode 100644 sw/airborne/modules/cam_control/gimbal_caddx_gm3.c
 create mode 100644 sw/airborne/modules/cam_control/gimbal_caddx_gm3.h
 delete mode 100644 sw/airborne/modules/cam_control/point.c
 delete mode 100644 sw/airborne/modules/cam_control/point.h

diff --git a/conf/joystick/attack3_joystick_raw.xml b/conf/joystick/attack3_joystick_raw.xml
new file mode 100644
index 0000000000..d4d65b056e
--- /dev/null
+++ b/conf/joystick/attack3_joystick_raw.xml
@@ -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>
diff --git a/conf/modules/cam_gimbal.xml b/conf/modules/cam_gimbal.xml
new file mode 100644
index 0000000000..a70466113a
--- /dev/null
+++ b/conf/modules/cam_gimbal.xml
@@ -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>
diff --git a/conf/modules/cam_point.xml b/conf/modules/cam_point.xml
deleted file mode 100644
index 2216315031..0000000000
--- a/conf/modules/cam_point.xml
+++ /dev/null
@@ -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>
diff --git a/conf/modules/cam_roll.xml b/conf/modules/cam_roll.xml
deleted file mode 100644
index 6b803aa812..0000000000
--- a/conf/modules/cam_roll.xml
+++ /dev/null
@@ -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>
diff --git a/conf/modules/cam_segment.xml b/conf/modules/cam_segment.xml
deleted file mode 100644
index cdfaee5596..0000000000
--- a/conf/modules/cam_segment.xml
+++ /dev/null
@@ -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>
diff --git a/conf/modules/gimbal_caddx_gm3.xml b/conf/modules/gimbal_caddx_gm3.xml
new file mode 100644
index 0000000000..6059a6815f
--- /dev/null
+++ b/conf/modules/gimbal_caddx_gm3.xml
@@ -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>
diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c
deleted file mode 100644
index bf5dcbcd1a..0000000000
--- a/sw/airborne/modules/cam_control/cam.c
+++ /dev/null
@@ -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
diff --git a/sw/airborne/modules/cam_control/cam.h b/sw/airborne/modules/cam_control/cam.h
deleted file mode 100644
index 4d15fcd7af..0000000000
--- a/sw/airborne/modules/cam_control/cam.h
+++ /dev/null
@@ -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
diff --git a/sw/airborne/modules/cam_control/cam_gimbal.c b/sw/airborne/modules/cam_control/cam_gimbal.c
new file mode 100644
index 0000000000..33e7cd6797
--- /dev/null
+++ b/sw/airborne/modules/cam_control/cam_gimbal.c
@@ -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
+}
+
diff --git a/sw/airborne/modules/cam_control/cam_gimbal.h b/sw/airborne/modules/cam_control/cam_gimbal.h
new file mode 100644
index 0000000000..0a0c8361a8
--- /dev/null
+++ b/sw/airborne/modules/cam_control/cam_gimbal.h
@@ -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
+
diff --git a/sw/airborne/modules/cam_control/cam_roll.c b/sw/airborne/modules/cam_control/cam_roll.c
deleted file mode 100644
index c3a75b4f2a..0000000000
--- a/sw/airborne/modules/cam_control/cam_roll.c
+++ /dev/null
@@ -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
diff --git a/sw/airborne/modules/cam_control/cam_roll.h b/sw/airborne/modules/cam_control/cam_roll.h
deleted file mode 100644
index fcd9e7a0c6..0000000000
--- a/sw/airborne/modules/cam_control/cam_roll.h
+++ /dev/null
@@ -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 */
diff --git a/sw/airborne/modules/cam_control/cam_segment.c b/sw/airborne/modules/cam_control/cam_segment.c
deleted file mode 100644
index 4e69f3f44c..0000000000
--- a/sw/airborne/modules/cam_control/cam_segment.c
+++ /dev/null
@@ -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;
-}
-
diff --git a/sw/airborne/modules/cam_control/cam_segment.h b/sw/airborne/modules/cam_control/cam_segment.h
deleted file mode 100644
index 4a00fce79c..0000000000
--- a/sw/airborne/modules/cam_control/cam_segment.h
+++ /dev/null
@@ -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
diff --git a/sw/airborne/modules/cam_control/gimbal_caddx_gm3.c b/sw/airborne/modules/cam_control/gimbal_caddx_gm3.c
new file mode 100644
index 0000000000..589b6a94f4
--- /dev/null
+++ b/sw/airborne/modules/cam_control/gimbal_caddx_gm3.c
@@ -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
+}
+
diff --git a/sw/airborne/modules/cam_control/gimbal_caddx_gm3.h b/sw/airborne/modules/cam_control/gimbal_caddx_gm3.h
new file mode 100644
index 0000000000..b7fe7f5bc0
--- /dev/null
+++ b/sw/airborne/modules/cam_control/gimbal_caddx_gm3.h
@@ -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
+
diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c
deleted file mode 100644
index 46e9a49080..0000000000
--- a/sw/airborne/modules/cam_control/point.c
+++ /dev/null
@@ -1,746 +0,0 @@
-/*
- * Copyright (C) 2005-2008  Arnold Schroeter
- * Modified and expanded to show the coordinates of where the camera is looking at
- * by Chris Efstathiou 23-Jan-2011 AD.
- *
- * 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 point.c
- *  \brief Determines camera pan and tilt angles.
- *
- * project:        Paparazzi
- * description:    Determines camera pan and tilt angles from
- *                 plane's and object's positions and plane's
- *                 pitch and roll angles. Software might be optimized
- *                 by removing multiplications with 0, it is left this
- *                 way for better understandabilty and changeability.
- *
- * author:         Arnold Schroeter, Martin Mueller
- *
- * hardware:
- *
- * The camera control is made of normal servos. Usually servos have a
- * turn angle of about 90°. This is changed electrically so that they
- * can do a 180°. It is achieved by adding two serial resistors at both
- * sides of the potentiometer (P1), one for increasing the usable angle
- * (R1) and the other for moving the middle position to a useful angle
- * (R2). Therefore a servo with a 270° potentiometer is needed. Very
- * small and light servos have 180° potentiometers, these do not allow
- * a 180° degrees sweep. Cut the outer two connections between the
- * potentiometer and the board to insert the resistors. The values for
- * R1 and R2 should be found out by testing as there might be serial
- * resistors on the servo board that affect the values. Start with
- * about 1/2 the value of P1 for R1 and change R1 until you get a
- * little more than 180° sweep. Then insert and modify R2 to set
- * neutral back to the middle position of the potentiometer.
- *
- *
- *                     ^
- *                    /
- *              ----------
- *      *-------I   /    I-------*
- *      I       ----------       I
- *      I         /     P1       I
- *      I         I              I
- *      I         I              I
- *     ---        I             ---
- *     I I        I             I I
- *     I I        I             I I
- *     I I        I             I I
- *     --- R1     I             --- R2
- *      I         I              I
- *      I         I              I
- *
- *
- */
-
-#include <math.h>
-#include "cam.h"
-#include "point.h"
-#include "autopilot.h"
-#include "generated/flight_plan.h"
-#include "modules/nav/common_nav.h"
-#include "modules/gps/gps.h"
-#include "math/pprz_geodetic_float.h"
-#include "state.h"
-
-typedef struct {
-  float fx;
-  float fy;
-  float fz;
-} VECTOR;
-
-typedef struct {
-  float fx1; float fx2; float fx3;
-  float fy1; float fy2; float fy3;
-  float fz1; float fz2; float fz3;
-} MATRIX;
-
-float cam_theta;
-float cam_phi;
-bool heading_positive = 0;
-float  memory_x, memory_y, memory_z;
-#if defined(SHOW_CAM_COORDINATES)
-float   cam_point_x;
-float   cam_point_y;
-uint16_t cam_point_distance_from_home;
-float cam_point_lon,  cam_point_lat;
-float distance_correction = 1;
-#endif
-
-void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
-void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC);
-
-/*******************************************************************
-; function name:   vSubtractVectors
-; description:     subtracts two vectors a = b - c
-; parameters:
-;*******************************************************************/
-void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
-{
-  svA->fx = svB.fx - svC.fx;
-  svA->fy = svB.fy - svC.fy;
-  svA->fz = svB.fz - svC.fz;
-}
-
-/*******************************************************************
-; function name:   vMultiplyMatrixByVector
-; description:     multiplies matrix by vector svA = smB * svC
-; parameters:
-;*******************************************************************/
-void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
-{
-  svA->fx = smB.fx1 * svC.fx  +  smB.fx2 * svC.fy  +  smB.fx3 * svC.fz;
-  svA->fy = smB.fy1 * svC.fx  +  smB.fy2 * svC.fy  +  smB.fy3 * svC.fz;
-  svA->fz = smB.fz1 * svC.fx  +  smB.fz2 * svC.fy  +  smB.fz3 * svC.fz;
-}
-
-/*******************************************************************
-; function name:   vPoint
-; description:     Transforms ground coordinate system into
-;                  plane's coordinate system via three rotations
-;                  and determines positions of camera servos.
-; parameters:      fPlaneNorth, fPlaneEast, fPlaneAltitude  plane's
-;                           position with respect to ground
-;                           in m (actually the units do not matter as
-;                           long as they are the same as for the object's
-;                           position)
-;                  fRollAngle  level=0; right wing down = positive values
-;                  fPitchAngle level=0; nose up = positive values
-;                           plane's pitch and roll angles
-;                           with respect to ground in radians
-;                  fYawAngle   north=0; right= positive values in radians
-;                           plane's yaw angle with respect to north
-;                  fObjectNorth, fObjectEast, fAltitude object's
-;                           position with respect to ground
-;                           in m (actually the units do not matter as
-;                           long as they are the same for the plane's
-;                           position)
-;                  fPan, fTilt angles for camera servos in radians,
-;                           pan is turn/left-right and tilt is down-up
-;                           in reference to the camera picture
-; camera mount:    The way the camera is mounted is given through a
-;                  define POINT_CAM_a_[_b] where a gives the mount
-;                  angle within the aircraft and b the angle when
-;                  viewing the direction of the first servo.
-;*******************************************************************/
-void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
-            float fRollAngle, float fPitchAngle, float fYawAngle,
-            float fObjectEast, float fObjectNorth, float fAltitude,
-            float *fPan, float *fTilt)
-{
-  static VECTOR svPlanePosition,
-         svObjectPosition,
-         svObjectPositionForPlane,
-         svObjectPositionForPlane2;
-
-  static VECTOR sv_cam_projection,
-         sv_cam_projection_buf;
-
-  static MATRIX smRotation;
-
-  /***        BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS)   ***/
-  /***        IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT            ***/
-  /***        THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC.                                     ***/
-  /***        IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED                ***/
-  if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC || cam_mode == CAM_MODE_WP_TARGET
-      || cam_mode == CAM_MODE_XY_TARGET) {
-
-    if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC) {
-
-      // protect acces to fbw state
-      pprz_mtx_lock(&fbw_state_mtx);
-      /*########################################  TILT CONTROL INPUT  #############################################*/
-#ifdef CAM_TILT_NEUTRAL
-
-#if defined(RADIO_TILT)
-      if ((*fbw_state).channels[RADIO_TILT] >= 0) {
-        cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ) *
-                    (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
-
-      } else {
-        cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MIN_PPRZ) *
-                    (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
-      }
-#elif defined(RADIO_PITCH)
-      if ((*fbw_state).channels[RADIO_PITCH] >= 0) {
-        cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MAX_PPRZ) *
-                    (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
-
-      } else {
-        cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MIN_PPRZ) *
-                    (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
-      }
-#else
-#error RADIO_TILT or RADIO_PITCH not defined.
-#endif
-
-#else   //#ifdef CAM_TILT_NEUTRAL
-
-#if defined(RADIO_TILT)
-      cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(
-                                              *fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ));
-#elif defined(RADIO_PITCH)
-      cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(
-                                              *fbw_state).channels[RADIO_PITCH] / (float)MAX_PPRZ));
-#else
-#error RADIO_TILT or RADIO_PITCH not defined.
-#endif
-
-#endif   //#ifdef CAM_TILT_NEUTRAL
-      /*########################################  END OF TILT CONTROL INPUT  ########################################*/
-
-      /*###########################################  PAN CONTROL INPUT  #############################################*/
-#ifdef CAM_PAN_NEUTRAL
-
-#if defined(RADIO_PAN)
-      if ((*fbw_state).channels[RADIO_PAN] >= 0) {
-        cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MAX_PPRZ) *
-                                               RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL));
-
-      } else {
-        cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MIN_PPRZ) *
-                                               RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL));
-      }
-#elif defined(RADIO_ROLL)
-      if ((*fbw_state).channels[RADIO_ROLL] >= 0) {
-        cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MAX_PPRZ) *
-                                               RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL));
-
-      } else {
-        cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MIN_PPRZ) *
-                                               RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL));
-      }
-#else
-#error RADIO_PAN or RADIO_ROLL not defined.
-#endif
-
-#else   //#ifdef CAM_PAN_NEUTRAL
-
-#if defined(RADIO_PAN)
-      cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /
-                                         (float)MAX_PPRZ));
-#elif defined(RADIO_ROLL)
-      cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /
-                                         (float)MAX_PPRZ));
-#else
-#error RADIO_PAN or RADIO_ROLL not defined.
-#endif
-
-#endif  //#ifdef CAM_PAN_NEUTRAL
-      /*########################################  END OF PAN CONTROL INPUT  #############################################*/
-      pprz_mtx_unlock(&fbw_state_mtx);
-
-      // Bound Pan and Tilt angles.
-      if (cam_theta > RadOfDeg(CAM_TILT_MAX)) {
-        cam_theta = RadOfDeg(CAM_TILT_MAX);
-
-      } else if (cam_theta < RadOfDeg(CAM_TILT_MIN)) { cam_theta = RadOfDeg(CAM_TILT_MIN); }
-
-      if (cam_phi > RadOfDeg(CAM_PAN_MAX)) {
-        cam_phi = RadOfDeg(CAM_PAN_MAX);
-
-      } else if (cam_phi < RadOfDeg(CAM_PAN_MIN)) { cam_phi = RadOfDeg(CAM_PAN_MIN); }
-
-
-      svPlanePosition.fx = fPlaneEast;
-      svPlanePosition.fy = fPlaneNorth;
-      svPlanePosition.fz = fPlaneAltitude;
-
-// FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES
-//cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT
-//cam_theta = RadOfDeg(70);     //  LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT
-
-      if (cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC) { // Not much to see after 80 degrees of tilt so stop tracking.
-        *fPan = cam_phi;
-        *fTilt = cam_theta;
-#ifdef SHOW_CAM_COORDINATES
-        cam_point_distance_from_home = 0;
-        cam_point_lon = 0;
-        cam_point_lat = 0;
-#endif
-        return;
-
-      } else {
-        sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta) * (fPlaneAltitude - ground_alt));
-        sv_cam_projection_buf.fy = svPlanePosition.fy;
-      }
-
-#if defined(WP_CAM_POINT)
-      sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a;
-#else
-      sv_cam_projection_buf.fz = ground_alt;
-#endif
-
-      /* distance between plane and camera projection */
-      vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition);
-
-      float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation.
-      if (heading_radians > RadOfDeg(180)) { heading_radians -= RadOfDeg(360); }
-      if (heading_radians < RadOfDeg(-180)) { heading_radians += RadOfDeg(360); }
-      //heading_radians += cam_theta;
-
-      /* camera pan angle correction, using a clockwise rotation */
-      smRotation.fx1 = (float)(cos(cam_phi));
-      smRotation.fx2 = (float)(sin(cam_phi));
-      smRotation.fx3 = 0.;
-      smRotation.fy1 = -smRotation.fx2;
-      smRotation.fy2 = smRotation.fx1;
-      smRotation.fy3 = 0.;
-      smRotation.fz1 = 0.;
-      smRotation.fz2 = 0.;
-      smRotation.fz3 = 1.;
-
-      vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection);
-
-      /* yaw correction using a counter clockwise rotation*/
-      smRotation.fx1 = (float)(cos(heading_radians));
-      smRotation.fx2 = -(float)(sin(heading_radians));
-      smRotation.fx3 = 0.;
-      smRotation.fy1 = -smRotation.fx2;
-      smRotation.fy2 = smRotation.fx1;
-      smRotation.fy3 = 0.;
-      smRotation.fz1 = 0.;
-      smRotation.fz2 = 0.;
-      smRotation.fz3 = 1.;
-
-      vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf);
-
-#if defined(RADIO_CAM_LOCK)
-      float radio_cam_lock = radio_control_get(RADIO_CAM_LOCK);
-      if ((radio_cam_lock > MAX_PPRZ / 2) && autopilot_get_mode() == AP_MODE_AUTO2) { cam_lock = true; }
-      if ((radio_cam_lock < MIN_PPRZ / 2) && autopilot_get_mode() == AP_MODE_AUTO2) { cam_lock = false; }
-#endif
-    // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
-    if (cam_lock == FALSE) {
-      fObjectEast = (fPlaneEast + sv_cam_projection.fx) ;
-        fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ;
-        fAltitude = ground_alt;
-        memory_x = fObjectEast;
-        memory_y = fObjectNorth;
-        memory_z = fAltitude;
-#if defined(WP_CAM_POINT)
-        waypoints[WP_CAM_POINT].x = fObjectEast;
-        waypoints[WP_CAM_POINT].y = fObjectNorth;
-        waypoints[WP_CAM_POINT].a = ground_alt;
-#endif
-#if defined(SHOW_CAM_COORDINATES)
-        cam_point_x = fObjectEast;
-        cam_point_y = fObjectNorth;
-
-        cam_point_distance_from_home = distance_correction * (uint16_t)(sqrt((cam_point_x * cam_point_x) +
-                                       (cam_point_y * cam_point_y)));
-
-        struct UtmCoor_f utm = *stateGetPositionUtm_f();
-        utm.east += sv_cam_projection.fx;
-        utm.north += sv_cam_projection.fy;
-        struct LlaCoor_f lla;
-        lla_of_utm_f(&lla, &utm);
-        cam_point_lon = lla.lon * (180 / M_PI);
-        cam_point_lat = lla.lat * (180 / M_PI);
-#endif
-
-      } else {
-        fObjectEast = memory_x;
-        fObjectNorth = memory_y;
-        fAltitude = memory_z;
-#if defined(WP_CAM_POINT)
-        waypoints[WP_CAM_POINT].x = fObjectEast;
-        waypoints[WP_CAM_POINT].y = fObjectNorth;
-        waypoints[WP_CAM_POINT].a = fAltitude;
-#endif
-      }
-
-      if (cam_mode == CAM_MODE_RC && cam_lock == 0) {
-      *fPan = cam_phi;
-      *fTilt = cam_theta;
-      return;
-    }
-
-#if defined(WP_CAM_POINT)
-    else {
-      waypoints[WP_CAM_POINT].x = fObjectEast;
-        waypoints[WP_CAM_POINT].y = fObjectNorth;
-        waypoints[WP_CAM_POINT].a = fAltitude;
-      }
-#endif
-      /***    END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT     ***/
-    } else {
-      /***    THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET           ***/
-#ifdef SHOW_CAM_COORDINATES
-      cam_point_distance_from_home = distance_correction * (uint16_t) fabs(((uint16_t)(sqrt((fObjectNorth * fObjectNorth) +
-                                     (fObjectEast * fObjectEast)))) -
-                                     ((uint16_t)(sqrt((fPlaneNorth * fPlaneNorth) + (fPlaneEast * fPlaneEast)))));
-
-      struct UtmCoor_f utm;
-      utm.east = nav_utm_east0 + fObjectEast;
-      utm.north = nav_utm_north0 + fObjectNorth;
-      utm.zone = nav_utm_zone0;
-      struct LlaCoor_f lla;
-      lla_of_utm_f(&lla, &utm);
-      cam_point_lon = lla.lon * (180 / M_PI);
-      cam_point_lat = lla.lat * (180 / M_PI);
-#endif
-
-
-#if defined(WP_CAM_POINT)
-      waypoints[WP_CAM_POINT].x = fObjectEast;
-      waypoints[WP_CAM_POINT].y = fObjectNorth;
-      waypoints[WP_CAM_POINT].a = fAltitude;
-#endif
-
-    }
-
-  }
-
-//************************************************************************************************
-//************************************************************************************************
-//************************************************************************************************
-//************************************************************************************************
-
-  /*
-  By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle)
-  to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly
-  applied to the rotation matrices (in radians).
-  In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left
-  and +90 is to the top (counterclockwise rotation).
-  When reading back the actual rotated coordinates sv_cam_projection.fx has the y coordinate and sv_cam_projection.fy has the x
-  represented on a circle in standard mathematical notation.
-  */
-  svPlanePosition.fx = fPlaneNorth;
-  svPlanePosition.fy = fPlaneEast;
-  svPlanePosition.fz = fPlaneAltitude;
-
-  svObjectPosition.fx = fObjectNorth;
-  svObjectPosition.fy = fObjectEast;
-  svObjectPosition.fz = fAltitude;
-
-  /* distance between plane and object */
-  vSubtractVectors(&svObjectPositionForPlane, svObjectPosition, svPlanePosition);
-
-  /* yaw */
-  smRotation.fx1 = (float)(cos(fYawAngle));
-  smRotation.fx2 = (float)(sin(fYawAngle));
-  smRotation.fx3 = 0.;
-  smRotation.fy1 = -smRotation.fx2;
-  smRotation.fy2 = smRotation.fx1;
-  smRotation.fy3 = 0.;
-  smRotation.fz1 = 0.;
-  smRotation.fz2 = 0.;
-  smRotation.fz3 = 1.;
-
-  vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation, svObjectPositionForPlane);
-
-  /* pitch */
-  smRotation.fx1 = (float)(cos(fPitchAngle));
-  smRotation.fx2 = 0.;
-  smRotation.fx3 = (float)(sin(fPitchAngle));
-  smRotation.fy1 = 0.;
-  smRotation.fy2 = 1.;
-  smRotation.fy3 = 0.;
-  smRotation.fz1 = -smRotation.fx3;
-  smRotation.fz2 = 0.;
-  smRotation.fz3 = smRotation.fx1;
-
-  vMultiplyMatrixByVector(&svObjectPositionForPlane, smRotation, svObjectPositionForPlane2);
-
-  /* roll */
-  smRotation.fx1 = 1.;
-  smRotation.fx2 = 0.;
-  smRotation.fx3 = 0.;
-  smRotation.fy1 = 0.;
-  smRotation.fy2 = (float)(cos(fRollAngle));
-  smRotation.fy3 = (float)(-sin(fRollAngle));
-  smRotation.fz1 = 0.;
-  smRotation.fz2 = -smRotation.fy3;
-  smRotation.fz3 = smRotation.fy2;
-
-  vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation, svObjectPositionForPlane);
-
-#ifdef POINT_CAM_PITCH
-
-  /*
-   * This is for one axis pitch camera mechanisms. The pitch servo neutral
-   * makes the camera look down, 90° is to the front and -90° is to the
-   * back. The pitch value is given through the tilt parameter.
-   * The camera picture is upright when looking in flight direction.
-   *
-   * tilt servo, looking from left:
-   *
-   *     plane front <-------------- plane back
-   *                      / I \
-   *                     /  I  \
-   *                   45°  I  -45°
-   *                        0°
-   *
-   * (should be hyperbolic, we use lines to make it better, the plane rolls
-   *  away from the object while flying towards it!)
-   *
-   */
-
-  /* fTilt =   0 -> camera looks down
-              90 -> camera looks forward
-             -90 -> camera looks backward
-  */
-#if 0 //we roll away anyways
-  *fTilt = (float)(atan2(svObjectPositionForPlane2.fx,
-                         sqrt(svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy
-                              + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz)
-                        ));
-#else
-  *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz));
-#endif
-
-  /* fPan is deactivated
-  */
-  *fPan = 0;
-#else
-#ifdef POINT_CAM_ROLL
-
-  /*
-   * This is for single axis roll camera mechanisms. The tilt servo neutral
-   * makes the camera look down, -90° is to the right and 90° is to the
-   * left.
-   * The camera picture is upright when looking to the right.
-   *
-   *
-   * tilt servo, looking from behind:
-   *
-   *     plane left --------------- plane right
-   *                     / I \
-   *                    /  I  \
-   *                  45°  I  -45°
-   *                       0°
-   *
-   */
-#if 1  // have to check if it helps
-  *fTilt = (float)(atan2(svObjectPositionForPlane2.fy,
-                         sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
-                              + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz)
-                        ));
-#else
-  *fTilt = (float)(atan2(svObjectPositionForPlane2.fy, -svObjectPositionForPlane2.fz));
-#endif
-
-  /* fPan is deactivated
-  */
-  *fPan = 0;
-#else
-#ifdef POINT_CAM_YAW_PITCH_NOSE
-
-  /*
-                   -45   0   45
-                      \  |  /
-                       \ | /
-                        \|/
-                      ##
-                      ##
-           _____________##______________
-  left tip|_____________________________|right wing tip
-                      ##
-                      ##
-                      ##
-                      ##
-                  ______##______
-                 |_____|_|_____|
-                        |
-  */
-
-#if defined(CAM_PAN_MODE) && CAM_PAN_MODE == 360
-  /* fixed to the plane*/
-  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, (svObjectPositionForPlane2.fx)));
-
-  *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
-                              + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy),
-                         -svObjectPositionForPlane2.fz
-                        ));
-
-  // I need to avoid oscillations around the 180 degree mark.
-  /*
-     if (*fPan > 0 && *fPan <= RadOfDeg(175)){ heading_positive = 1; }
-     if (*fPan < 0 && *fPan >= RadOfDeg(-175)){ heading_positive = 0; }
-
-     if (*fPan > RadOfDeg(175) && heading_positive == 0){
-        *fPan = RadOfDeg(-180);
-
-     }else if (*fPan < RadOfDeg(-175) && heading_positive){
-              *fPan = RadOfDeg(180);
-              heading_positive = 0;
-           }
-  */
-#else
-  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, fabs(svObjectPositionForPlane2.fx)));
-
-  *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
-                              + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy),
-                         -svObjectPositionForPlane2.fz
-                        ));
-
-  if (svObjectPositionForPlane2.fx < 0) {
-    *fPan = -*fPan;
-    *fTilt = -*fTilt;
-  }
-
-  // I need to avoid oscillations around the 180 degree mark.
-  /*
-     if (*fPan > 0 && *fPan <= RadOfDeg(85)){ heading_positive = 1; }
-     if (*fPan < 0 && *fPan >= RadOfDeg(-85)){ heading_positive = 0; }
-
-     if (*fPan > RadOfDeg(85) && heading_positive == 0){
-        *fPan = RadOfDeg(-90);
-
-     }else if (*fPan < RadOfDeg(-85) && heading_positive){
-              *fPan = RadOfDeg(90);
-              heading_positive = 0;
-           }
-  */
-#endif
-
-#else
-#ifdef POINT_CAM_YAW_PITCH
-
-  /*
-   * This is for two axes pan/tilt camera mechanisms. The default is to
-   * circle clockwise so view is right. The pan servo neutral makes
-   * the camera look to the right with 0° given, 90° is to the back and
-   * -90° is to the front. The tilt servo neutral makes the camera look
-   * down with 0° given, 90° is to the right and -90° is to the left (all
-   * values are used in radian in the software). If the camera looks to
-   * the right side of the plane, the picture is upright. It is upside
-   * down when looking to the left. That is corrected with the MPEG
-   * decoding software on the laptop by mirroring. The pan servo is fixed
-   * in the plane and the tilt servo is moved by the pan servo and moves
-   * the camera.
-   *
-   *
-   * pan servo, tilt set to 90°, looking from top:
-   *
-   *   plane front
-   *
-   *       ^
-   *       I
-   *       I  45°
-   *       I /
-   *       I/
-   *       I------- 0°
-   *       I\
-   *       I \
-   *       I  -45°
-   *       I
-   *
-   *   plane back
-   *
-   *
-   * tilt servo, pan set to 0°, looking from back:
-   *
-   *     plane left --------------- plane right
-   *                     / I \
-   *                    /  I  \
-   *                 -45°  I   45°
-   *                       0°
-   *
-   */
-
-  /* fPan =   0  -> camera looks along the wing
-             90  -> camera looks in flight direction
-            -90  -> camera looks backwards
-  */
-  /* fixed to the plane*/
-  *fPan = (float)(atan2(svObjectPositionForPlane2.fx,
-                        fabs(svObjectPositionForPlane2.fy)));// Or is it the opposite??? (CEF)
-  // (CEF) It turned out that Object_North is loaded with x and Object_East with y (reverse). See line #155
-  // this means that:
-  // *fPan = (float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fy)); // makes the camera 0 to look to the nose?
-
-  /* fTilt =   0  -> camera looks down
-              90  -> camera looks into right hemisphere
-             -90  -> camera looks into left hemispere
-     actually the camera always looks more or less downwards, but never upwards
-  */
-  *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
-                              + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy),
-                         -svObjectPositionForPlane2.fz
-                        ));
-  if (svObjectPositionForPlane2.fy < 0) {
-    *fPan = -*fPan;
-    *fTilt = -*fTilt;
-  }
-
-#else
-#ifdef POINT_CAM_PITCH_ROLL
-
-  /*
-   * This is for another two axes camera mechanisms. The tilt servo is fixed to
-   * the fuselage and moves the pan servo.
-   *
-   * tilt servo, looking from left:
-   *
-   *    plane front <--------------- plane back
-   *                      / I \
-   *                     /  I  \
-   *                   45°  I  -45°
-   *                        0°
-   *
-   *
-   * pan servo, looking from back:
-   *
-   *     plane left --------------- plane right
-   *                     / I \
-   *                    /  I  \
-   *                  45°  I  -45°
-   *                       0°
-   *
-   */
-
-  *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz));
-
-  *fPan  = (float)(atan2(-svObjectPositionForPlane2.fy,
-                         sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
-                              + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz)
-                        ));
-
-#else
-#error at least one POINT_CAM_* camera mount has to be defined!
-#endif
-#endif
-#endif
-#endif
-#endif
-}
diff --git a/sw/airborne/modules/cam_control/point.h b/sw/airborne/modules/cam_control/point.h
deleted file mode 100644
index 584d926bdd..0000000000
--- a/sw/airborne/modules/cam_control/point.h
+++ /dev/null
@@ -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 */