diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index ed812e9dac..11ccf464c0 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -3,12 +3,9 @@ - - + + + @@ -50,21 +47,21 @@ - - - + + + - - - - + + + + - + @@ -74,11 +71,6 @@ - - @@ -264,25 +256,18 @@ -
- - - - - +
+ -
-
- - - +
+ + + + +
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 0298610a0a..03413695ea 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -2,7 +2,7 @@ - + @@ -37,12 +37,6 @@ -
- - - -
- @@ -51,8 +45,8 @@ - + @@ -62,6 +56,12 @@ +
+ + + +
+ @@ -213,18 +213,19 @@
-
+
- - - - - - - - - + + + +
+ +
+ + + +
diff --git a/conf/joystick/xbox_booz_cam.xml b/conf/joystick/xbox_rotorcraft_cam.xml similarity index 80% rename from conf/joystick/xbox_booz_cam.xml rename to conf/joystick/xbox_rotorcraft_cam.xml index 5eecdc472e..5e14acba4d 100644 --- a/conf/joystick/xbox_booz_cam.xml +++ b/conf/joystick/xbox_rotorcraft_cam.xml @@ -4,7 +4,7 @@ - + diff --git a/conf/messages.xml b/conf/messages.xml index 51db383031..b2752570b5 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1394,8 +1394,8 @@ - - + + @@ -2177,7 +2177,7 @@ - + diff --git a/conf/modules/booz_cam.xml b/conf/modules/booz_cam.xml deleted file mode 100644 index 48d9baaa98..0000000000 --- a/conf/modules/booz_cam.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - Camera control for BOOZ board - - -
- -
- - - - - - - - - diff --git a/conf/modules/rotorcraft_cam.xml b/conf/modules/rotorcraft_cam.xml new file mode 100644 index 0000000000..05142e146d --- /dev/null +++ b/conf/modules/rotorcraft_cam.xml @@ -0,0 +1,33 @@ + + + + + + Camera control for rotorcraft. + The camera is controled by the heading of the vehicle for pan and can be controlled by a servo for tilt if defined. + +
+ + + + + + + + + + +
+ +
+ +
+ + + + + + + + + diff --git a/conf/settings/modules/booz_cam.xml b/conf/settings/modules/rotorcraft_cam.xml similarity index 54% rename from conf/settings/modules/booz_cam.xml rename to conf/settings/modules/rotorcraft_cam.xml index 4b7c08e05d..17ae9b50b4 100644 --- a/conf/settings/modules/booz_cam.xml +++ b/conf/settings/modules/rotorcraft_cam.xml @@ -2,7 +2,7 @@ - + @@ -10,12 +10,12 @@ - - + + - + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index c55ee17611..2376ac8749 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -13,7 +13,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index ba7d1f08a1..d54fcea817 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -737,9 +737,9 @@ } #if USE_CAM -#define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) DOWNLINK_SEND_BOOZ2_CAM(_trans, _dev,&booz_cam_tilt,&booz_cam_pan); +#define PERIODIC_SEND_ROTORCRAFT_CAM(_trans, _dev) DOWNLINK_SEND_ROTORCRAFT_CAM(_trans, _dev,&rotorcraft_cam_tilt,&rotorcraft_cam_pan); #else -#define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) {} +#define PERIODIC_SEND_ROTORCRAFT_CAM(_trans, _dev) {} #endif #define PERIODIC_SEND_ROTORCRAFT_TUNE_HOVER(_trans, _dev) { \ diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c deleted file mode 100644 index ddd5dd3451..0000000000 --- a/sw/airborne/modules/cam_control/booz_cam.c +++ /dev/null @@ -1,149 +0,0 @@ -/* - * $Id: $ - * - * Copyright (C) 2009 Gautier Hattenberger , - * 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. - */ - -#include "cam_control/booz_cam.h" -#include "modules/core/booz_pwm_arch.h" -#include "state.h" -#include "firmwares/rotorcraft/navigation.h" -#include "generated/flight_plan.h" -#include "std.h" - -uint8_t booz_cam_mode; - -// Tilt definition -int16_t booz_cam_tilt; -int16_t booz_cam_tilt_pwm; -#ifdef BOOZ_CAM_TILT_NEUTRAL -#ifndef BOOZ_CAM_TILT_MIN -#define BOOZ_CAM_TILT_MIN BOOZ_CAM_TILT_NEUTRAL -#endif -#ifndef BOOZ_CAM_TILT_MAX -#define BOOZ_CAM_TILT_MAX BOOZ_CAM_TILT_NEUTRAL -#endif -#define BOOZ_CAM_USE_TILT 1 -#endif - -// Pan definition -int16_t booz_cam_pan; -#ifdef BOOZ_CAM_PAN_NEUTRAL -#ifndef BOOZ_CAM_PAN_MIN -#define BOOZ_CAM_PAN_MIN BOOZ_CAM_PAN_NEUTRAL -#endif -#ifndef BOOZ_CAM_PAN_MAX -#define BOOZ_CAM_PAN_MAX BOOZ_CAM_PAN_NEUTRAL -#endif -#define BOOZ_CAM_USE_PAN 1 -#endif - -#if defined BOOZ_CAM_TILT_ANGLE_MIN && defined BOOZ_CAM_TILT_ANGLE_MAX && defined BOOZ_CAM_USE_TILT -#define CAM_TA_MIN ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MIN) -#define CAM_TA_MAX ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MAX) -#define BOOZ_CAM_USE_TILT_ANGLES 1 -#endif - -// PWM definition -#ifndef BOOZ_CAM_SetPwm -#define BOOZ_CAM_SetPwm(_v) BoozSetPwmValue(_v) -#endif - -#ifndef BOOZ_CAM_DEFAULT_MODE -#define BOOZ_CAM_DEFAULT_MODE BOOZ_CAM_MODE_NONE -#endif - -void booz_cam_init(void) { - booz_cam_SetCamMode(BOOZ_CAM_DEFAULT_MODE); -#ifdef BOOZ_CAM_USE_TILT - booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; - BOOZ_CAM_SetPwm(booz_cam_tilt_pwm); - booz_cam_tilt = 0; -#else - booz_cam_tilt_pwm = 1500; - booz_cam_tilt = 0; -#endif -#ifdef BOOZ_CAM_USE_PAN - booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL; -#else - booz_cam_pan = 0; -#endif -} - -#define D_TILT (BOOZ_CAM_TILT_MAX - BOOZ_CAM_TILT_MIN) -#define CT_MIN Min(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX) -#define CT_MAX Max(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX) -#define CP_MIN Min(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX) -#define CP_MAX Max(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX) - -void booz_cam_periodic(void) { - - switch (booz_cam_mode) { - case BOOZ_CAM_MODE_NONE: -#ifdef BOOZ_CAM_USE_TILT - booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; -#endif -#ifdef BOOZ_CAM_USE_PAN - booz_cam_pan = stateGetNedToBodyEulers_i()->psi; -#endif - break; - case BOOZ_CAM_MODE_MANUAL: -#ifdef BOOZ_CAM_USE_TILT - Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); -#endif - break; - case BOOZ_CAM_MODE_HEADING: -#ifdef BOOZ_CAM_USE_TILT_ANGLES - Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX); - booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); - Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); -#endif -#ifdef BOOZ_CAM_USE_PAN - Bound(booz_cam_pan, CP_MIN, CP_MAX); - nav_heading = booz_cam_pan; -#endif - break; - case BOOZ_CAM_MODE_WP: -#ifdef WP_CAM - { - struct Int32Vect2 diff; - VECT2_DIFF(diff, waypoints[WP_CAM], *stateGetPositionEnu_i()); - INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); - INT32_ATAN2(booz_cam_pan,diff.x,diff.y); - nav_heading = booz_cam_pan; -#ifdef BOOZ_CAM_USE_TILT_ANGLES - int32_t dist, height; - INT32_VECT2_NORM(dist, diff); - height = (waypoints[WP_CAM].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; - INT32_ATAN2(booz_cam_tilt, height, dist); - Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); - booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); - Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); -#endif - } -#endif - break; - } -#ifdef BOOZ_CAM_USE_TILT - BOOZ_CAM_SetPwm(booz_cam_tilt_pwm); -#endif -} - diff --git a/sw/airborne/modules/cam_control/booz_cam.h b/sw/airborne/modules/cam_control/booz_cam.h deleted file mode 100644 index 2a1fbfb0d8..0000000000 --- a/sw/airborne/modules/cam_control/booz_cam.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * $Id: $ - * - * Copyright (C) 2009 Gautier Hattenberger , - * 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 BOOZ_CAM_H -#define BOOZ_CAM_H - -#include "generated/airframe.h" -#include "math/pprz_algebra_int.h" -#include "std.h" -#include "led.h" - -#define BOOZ_CAM_MODE_NONE 0 -#define BOOZ_CAM_MODE_MANUAL 1 -#define BOOZ_CAM_MODE_HEADING 2 -#define BOOZ_CAM_MODE_WP 3 - -// Warning: -// LED_ON set GPIO low -// LED_OFF set GPIO high -#ifndef BOOZ_CAM_ON -#define BOOZ_CAM_ON LED_OFF(CAM_SWITCH_LED) -#endif -#ifndef BOOZ_CAM_OFF -#define BOOZ_CAM_OFF LED_ON(CAM_SWITCH_LED) -#endif - -extern uint8_t booz_cam_mode; - -extern int16_t booz_cam_tilt; -extern int16_t booz_cam_pan; -extern int16_t booz_cam_tilt_pwm; - -extern void booz_cam_init(void); -extern void booz_cam_periodic(void); - -#define booz_cam_SetCamMode(_v) { \ - booz_cam_mode = _v; \ - if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { BOOZ_CAM_OFF; } \ - else { BOOZ_CAM_ON; } \ -} - -#define BOOZ_CAM_STICK_TILT_INC (ANGLE_BFP_OF_REAL(RadOfDeg(10.))/127.) -#define BOOZ_CAM_STICK_PAN_INC (ANGLE_BFP_OF_REAL(RadOfDeg(20.))/127.) - -#define BOOZ_CAM_STICK_PARSE(_dl_buffer) { \ - booz_cam_tilt += (int16_t)(BOOZ_CAM_STICK_TILT_INC*(float)DL_BOOZ_CAM_STICK_tilt(_dl_buffer)); \ - booz_cam_pan += (int16_t)(BOOZ_CAM_STICK_PAN_INC*(float)DL_BOOZ_CAM_STICK_pan(dl_buffer)); \ - INT32_COURSE_NORMALIZE(booz_cam_pan); \ -} - -#endif /* BOOZ2_CAM_H */ - diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c new file mode 100644 index 0000000000..0df7e0ba3f --- /dev/null +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2009-2012 Gautier Hattenberger , + * 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. + */ + +#include "modules/cam_control/rotorcraft_cam.h" + +#include "subsystems/actuators.h" +#include "state.h" +#include "firmwares/rotorcraft/navigation.h" +#include "std.h" + +uint8_t rotorcraft_cam_mode; + +#define _SERVO_PARAM(_s,_p) SERVO_ ## _s ## _ ## _p +#define SERVO_PARAM(_s,_p) _SERVO_PARAM(_s,_p) + +// Tilt definition +int16_t rotorcraft_cam_tilt; +int16_t rotorcraft_cam_tilt_pwm; +#if ROTORCRAFT_CAM_USE_TILT +#define ROTORCRAFT_CAM_TILT_NEUTRAL SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, NEUTRAL) +#define ROTORCRAFT_CAM_TILT_MIN SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MIN) +#define ROTORCRAFT_CAM_TILT_MAX SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MAX) +#define D_TILT (ROTORCRAFT_CAM_TILT_MAX - ROTORCRAFT_CAM_TILT_MIN) +#define CT_MIN Min(CAM_TA_MIN, CAM_TA_MAX) +#define CT_MAX Max(CAM_TA_MIN, CAM_TA_MAX) +#endif + +// Pan definition +int16_t rotorcraft_cam_pan; +#define ROTORCRAFT_CAM_PAN_MIN 0 +#define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI + +void rotorcraft_cam_init(void) { + rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE); +#if ROTORCRAFT_CAM_USE_TILT + rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; + ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); +#else + rotorcraft_cam_tilt_pwm = 1500; +#endif + rotorcraft_cam_tilt = 0; + rotorcraft_cam_pan = 0; +} + +void rotorcraft_cam_periodic(void) { + + switch (rotorcraft_cam_mode) { + case ROTORCRAFT_CAM_MODE_NONE: +#if ROTORCRAFT_CAM_USE_TILT + rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; +#endif +#if ROTORCRAFT_CAM_USE_PAN + rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi; +#endif + break; + case ROTORCRAFT_CAM_MODE_MANUAL: + // nothing to do here, just apply tilt pwm at the end + break; + case ROTORCRAFT_CAM_MODE_HEADING: +#if ROTORCRAFT_CAM_USE_TILT_ANGLES + Bound(rotorcraft_cam_tilt,CT_MIN,CT_MAX); + rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); +#endif +#if ROTORCRAFT_CAM_USE_PAN + INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); + nav_heading = rotorcraft_cam_pan; +#endif + break; + case ROTORCRAFT_CAM_MODE_WP: +#ifdef ROTORCRAFT_CAM_TRACK_WP + { + struct Int32Vect2 diff; + VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i()); + INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); + INT32_ATAN2(rotorcraft_cam_pan,diff.x,diff.y); + nav_heading = rotorcraft_cam_pan; +#if ROTORCRAFT_CAM_USE_TILT_ANGLES + int32_t dist, height; + INT32_VECT2_NORM(dist, diff); + height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; + INT32_ATAN2(rotorcraft_cam_tilt, height, dist); + Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); + rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); +#endif + } +#endif + break; + } +#if ROTORCRAFT_CAM_USE_TILT + ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); +#endif +} + diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.h b/sw/airborne/modules/cam_control/rotorcraft_cam.h new file mode 100644 index 0000000000..1b2babfe80 --- /dev/null +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.h @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2009-2012 Gautier Hattenberger , + * 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. + */ + +/** Camera control module for rotorcraft. + * + * The camera is controled by the heading of the vehicle for pan + * and can be controlled by a servo for tilt if defined. + * + * Four modes: + * - NONE: no control + * - MANUAL: the servo position is set with PWM + * - HEADING: the servo position and the heading of the rotorcraft are set with angles + * - WP: the camera is tracking a waypoint (Default: CAM) + * + * The CAM_SWITCH can be used to power the camera in normal modes + * and disable it when in NONE mode + */ + +#ifndef ROTORCRAFT_CAM_H +#define ROTORCRAFT_CAM_H + +#include "generated/airframe.h" +#include "generated/flight_plan.h" +#include "math/pprz_algebra_int.h" +#include "std.h" +#include "led.h" + +#define ROTORCRAFT_CAM_MODE_NONE 0 +#define ROTORCRAFT_CAM_MODE_MANUAL 1 +#define ROTORCRAFT_CAM_MODE_HEADING 2 +#define ROTORCRAFT_CAM_MODE_WP 3 + +/** Default mode is NONE. */ +#ifndef ROTORCRAFT_CAM_DEFAULT_MODE +#define ROTORCRAFT_CAM_DEFAULT_MODE ROTORCRAFT_CAM_MODE_NONE +#endif + +/** Cam power control. + * By default CAM_SWITCH is used + * Warning: + * LED_ON set GPIO low on some boards (lpc) + * LED_OFF set GPIO high on some boards (lpc) + */ +#ifndef ROTORCRAFT_CAM_ON +#define ROTORCRAFT_CAM_ON LED_OFF(CAM_SWITCH_LED) +#endif +#ifndef ROTORCRAFT_CAM_OFF +#define ROTORCRAFT_CAM_OFF LED_ON(CAM_SWITCH_LED) +#endif + +/** Cam tilt control. + * By default use tilt control if a servo is assigned + */ +#ifdef ROTORCRAFT_CAM_TILT_SERVO +#define ROTORCRAFT_CAM_USE_TILT 1 +#else +#define ROTORCRAFT_CAM_USE_TILT 0 +#endif + +/** Use angles for tilt in HEADING and WP modes. + */ +#if defined ROTORCRAFT_CAM_TILT_ANGLE_MIN && defined ROTORCRAFT_CAM_TILT_ANGLE_MAX && defined ROTORCRAFT_CAM_USE_TILT +#define CAM_TA_MIN ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_TILT_ANGLE_MIN) +#define CAM_TA_MAX ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_TILT_ANGLE_MAX) +#define ROTORCRAFT_CAM_USE_TILT_ANGLES 1 +#endif + +/** Cam pan control. + * By default use pan control (heading) + */ +#ifndef ROTORCRAFT_CAM_USE_PAN +#define ROTORCRAFT_CAM_USE_PAN 1 +#endif + +/** WP control. + * By default use WP_CAM waypoint if defined + */ +#ifndef ROTORCRAFT_CAM_TRACK_WP +#ifdef WP_CAM +#define ROTORCRAFT_CAM_TRACK_WP WP_CAM +#endif +#endif + +extern uint8_t rotorcraft_cam_mode; + +extern int16_t rotorcraft_cam_tilt; +extern int16_t rotorcraft_cam_pan; +extern int16_t rotorcraft_cam_tilt_pwm; + +extern void rotorcraft_cam_init(void); +extern void rotorcraft_cam_periodic(void); + +/** Set camera mode. + * Camera is powered down in NONE mode if CAM_{ON|OFF} are defined + */ +#define rotorcraft_cam_SetCamMode(_v) { \ + rotorcraft_cam_mode = _v; \ + if (rotorcraft_cam_mode == ROTORCRAFT_CAM_MODE_NONE) { ROTORCRAFT_CAM_OFF; } \ + else { ROTORCRAFT_CAM_ON; } \ +} + +/** Cam control from datalink message. + * camera tilt and pan are incremented by STICK_TILT_INC and STICK_PAN_INC + * when maximum command is received from the stick + */ +#ifndef ROTORCRAFT_CAM_STICK_TILT_INC +#define ROTORCRAFT_CAM_STICK_TILT_INC RadOfDeg(10.) +#endif +#ifndef ROTORCRAFT_CAM_STICK_PAN_INC +#define ROTORCRAFT_CAM_STICK_PAN_INC RadOfDeg(20.) +#endif + +#define ROTORCRAFT_CAM_STICK_PARSE(_dl_buffer) { \ + rotorcraft_cam_tilt += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_TILT_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_tilt(_dl_buffer)); \ + rotorcraft_cam_pan += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_PAN_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_pan(dl_buffer)); \ + INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); \ +} + +#endif /* ROTORCRAFT_CAM_H */ + diff --git a/sw/airborne/subsystems/actuators.h b/sw/airborne/subsystems/actuators.h index f742d02281..04d90811b6 100644 --- a/sw/airborne/subsystems/actuators.h +++ b/sw/airborne/subsystems/actuators.h @@ -49,7 +49,8 @@ extern int16_t actuators[ACTUATORS_NB]; * @param _n actuators name as given in airframe file, servos section * @param _v new actuator's value */ -#define ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v) +#define _ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v) +#define ActuatorSet(_n, _v) _ActuatorSet(_n, _v) #endif /* ACTUATORS_NB */