mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[actuators] update, rename and clean rotorcraft cam control
This commit is contained in:
Gautier Hattenberger
@@ -3,12 +3,9 @@
|
||||
<airframe name="Blender">
|
||||
|
||||
<modules main_freq="512">
|
||||
<!--load name="booz_pwm.xml">
|
||||
<define name="USE_PWM0"/>
|
||||
</load>
|
||||
<load name="booz_drop.xml"/>
|
||||
<load name="booz_cam.xml"/-->
|
||||
<load name="sonar_maxbotix_booz.xml"/>
|
||||
<load name="servo_switch.xml"/>
|
||||
<load name="rotorcraft_cam.xml"/>
|
||||
<!--load name="sonar_maxbotix_booz.xml"/-->
|
||||
<!--load name="adc_generic.xml">
|
||||
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
|
||||
</load-->
|
||||
@@ -50,21 +47,21 @@
|
||||
<define name="TRIM_R" value="-100"/>
|
||||
<define name="NB_MOTOR" value="4"/>
|
||||
<define name="SCALE" value="256"/>
|
||||
<define name="ROLL_COEF" value="{ 0, 0, -256, 256}"/>
|
||||
<define name="PITCH_COEF" value="{ 256, -256, 0, 0}"/>
|
||||
<define name="YAW_COEF" value="{ -256, -256, 256, 256}"/>
|
||||
<define name="ROLL_COEF" value="{ 0, -256, 0, 256}"/>
|
||||
<define name="PITCH_COEF" value="{ 256, 0, -256, 0}"/>
|
||||
<define name="YAW_COEF" value="{ -256, 256, -256, 256}"/>
|
||||
<define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/>
|
||||
</section>
|
||||
|
||||
<servos driver="Skiron">
|
||||
<servo name="FRONT" no="0" min="20" neutral="20" max="255"/>
|
||||
<servo name="BACK" no="2" min="20" neutral="20" max="255"/>
|
||||
<servo name="RIGHT" no="1" min="20" neutral="20" max="255"/>
|
||||
<servo name="LEFT" no="3" min="20" neutral="20" max="255"/>
|
||||
<servo name="FRONT" no="0" min="0" neutral="20" max="255"/>
|
||||
<servo name="RIGHT" no="1" min="0" neutral="20" max="255"/>
|
||||
<servo name="BACK" no="2" min="0" neutral="20" max="255"/>
|
||||
<servo name="LEFT" no="3" min="0" neutral="20" max="255"/>
|
||||
</servos>
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="DROP" no="5" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="DROP" no="0" min="1000" neutral="1500" max="2000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
@@ -74,11 +71,6 @@
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<!--section name="ACTUATORS_SKIRON" prefix="ACTUATORS_SKIRON_">
|
||||
<define name="NB" value="4"/>
|
||||
<define name="IDX" value="{ SERVO_FRONT, SERVO_BACK, SERVO_RIGHT, SERVO_LEFT }"/>
|
||||
</section-->
|
||||
|
||||
<command_laws>
|
||||
<call fun="supervision_run(autopilot_motors_on,FALSE,values)"/>
|
||||
<set servo="FRONT" value="supervision.commands[SERVO_FRONT]"/>
|
||||
@@ -264,25 +256,18 @@
|
||||
<define name="BOOZ_FMS_TIMEOUT" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="CAM" prefix="BOOZ_CAM_">
|
||||
<!--define name="TILT_NEUTRAL" value="1500"/>
|
||||
<define name="TILT_MIN" value="1060"/>
|
||||
<define name="TILT_MAX" value="2250"/>
|
||||
<define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
|
||||
<define name="TILT_ANGLE_MAX" value=" 0." unit="deg"/-->
|
||||
<define name="PAN_NEUTRAL" value="0"/>
|
||||
<define name="PAN_MIN" value="0"/>
|
||||
<define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
|
||||
<define name="DEFAULT_MODE" value="BOOZ_CAM_MODE_MANUAL"/>
|
||||
<section name="CAM" prefix="ROTORCRAFT_CAM_">
|
||||
<define name="DEFAULT_MODE" value="ROTORCRAFT_CAM_MODE_MANUAL"/>
|
||||
<define name="ON" value="{}"/>
|
||||
<define name="OFF" value="{}"/>
|
||||
<!--define name="SetPwm(_v)" value="Booz2SetPwm1Value(_v)"/-->
|
||||
</section>
|
||||
|
||||
<section name="DROP">
|
||||
<define name="DROP_SERVO_CLOSED" value="2000"/>
|
||||
<define name="DROP_SERVO_OPEN" value="1000"/>
|
||||
<define name="BoozDropPwm(_v)" value="BoozSetPwm0Value(_v)"/>
|
||||
<section name="SERVO_SWITCH">
|
||||
<define name="SERVO_SWITCH_SERVO" value="DROP"/>
|
||||
<define name="SERVO_SWITCH_ON_VALUE" value="SERVO_DROP_MIN"/>
|
||||
<define name="SERVO_SWITCH_OFF_VALUE" value="SERVO_DROP_MAX"/>
|
||||
<define name="DropOpen()" value="ServoSwitchOn()"/>
|
||||
<define name="DropClose()" value="ServoSwitchOff()"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
<modules main_freq="512">
|
||||
<load name="servo_switch.xml"/>
|
||||
<!--load name="booz_cam.xml"/-->
|
||||
<load name="rotorcraft_cam.xml"/>
|
||||
<!--load name="sonar_maxbotix_booz.xml"/-->
|
||||
<!--load name="adc_generic_booz.xml"/-->
|
||||
<!--load name="sys_mon.xml"/-->
|
||||
@@ -37,12 +37,6 @@
|
||||
<subsystem name="ins" type="hff"/>
|
||||
</firmware>
|
||||
|
||||
<section name="TRIM">
|
||||
<define name="TRIM_ROLL" value="0"/>
|
||||
<define name="TRIM_PITCH" value="6"/>
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
</section>
|
||||
|
||||
<servos driver="Asctec">
|
||||
<servo name="PITCH" no="0" min="-100" neutral="0" max="100"/>
|
||||
<servo name="ROLL" no="1" min="-100" neutral="0" max="100"/>
|
||||
@@ -51,8 +45,8 @@
|
||||
</servos>
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="CAM" no="5" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="SWITCH" no="0" min="1060" neutral="1500" max="2120"/>
|
||||
<servo name="CAM" no="1" min="1000" neutral="1500" max="2300"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
@@ -62,6 +56,12 @@
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<section name="TRIM">
|
||||
<define name="TRIM_ROLL" value="0"/>
|
||||
<define name="TRIM_PITCH" value="6"/>
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<set servo="PITCH" value="@PITCH - TRIM_PITCH"/>
|
||||
<set servo="ROLL" value="@ROLL - TRIM_ROLL"/>
|
||||
@@ -213,18 +213,19 @@
|
||||
<define name="BOOZ_FMS_TIMEOUT" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="CAM" prefix="BOOZ_CAM_">
|
||||
<section name="CAM" prefix="ROTORCRAFT_CAM_">
|
||||
<define name="ON" value="LED_ON(CAM_SWITCH_LED)"/>
|
||||
<define name="OFF" value="LED_OFF(CAM_SWITCH_LED)"/>
|
||||
<define name="TILT_NEUTRAL" value="1500"/>
|
||||
<define name="TILT_MAX" value="1000"/>
|
||||
<define name="TILT_MIN" value="2300"/>
|
||||
<define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
|
||||
<define name="TILT_ANGLE_MAX" value=" 10." unit="deg"/>
|
||||
<define name="PAN_NEUTRAL" value="0"/>
|
||||
<define name="PAN_MIN" value="0"/>
|
||||
<define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
|
||||
<define name="SetPwm(_v)" value="BoozSetPwm1Value(_v)"/>
|
||||
<define name="TILT_SERVO" value="CAM"/>
|
||||
<define name="TILT_ANGLE_MAX" value="-90." unit="deg"/>
|
||||
<define name="TILT_ANGLE_MIN" value=" 10." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="SERVO_SWITCH">
|
||||
<define name="SERVO_SWITCH_ON_VALUE" value="SERVO_SWITCH_MIN"/>
|
||||
<define name="SERVO_SWITCH_OFF_VALUE" value="SERVO_SWITCH_MAX"/>
|
||||
<define name="DropOpen()" value="ServoSwitchOn()"/>
|
||||
<define name="DropClose()" value="ServoSwitchOff()"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
<axis index="7" name="tilt"/>
|
||||
</input>
|
||||
<messages period="0.1">
|
||||
<message class="datalink" name="BOOZ_CAM_STICK">
|
||||
<message class="datalink" name="ROTORCRAFT_CAM_STICK">
|
||||
<field name="tilt" value="-tilt"/>
|
||||
<field name="pan" value="pan"/>
|
||||
</message>
|
||||
+3
-3
@@ -1394,8 +1394,8 @@
|
||||
<field name="var" type="float"/>
|
||||
</message>
|
||||
|
||||
<message name="BOOZ2_CAM" id="168">
|
||||
<field name="tilt" type="int16" unit="us"/>
|
||||
<message name="ROTORCRAFT_CAM" id="168">
|
||||
<field name="tilt" type="int16" unit="2^12rad" alt_unit="deg" alt_unit_coef="0.0139882"/>
|
||||
<field name="pan" type="int16" unit="2^12rad" alt_unit="deg" alt_unit_coef="0.0139882"/>
|
||||
</message>
|
||||
|
||||
@@ -2177,7 +2177,7 @@
|
||||
<field name="yd" type="float" unit="m"/>
|
||||
</message>
|
||||
|
||||
<message name="BOOZ_CAM_STICK" id="152" link="forwarded">
|
||||
<message name="ROTORCRAFT_CAM_STICK" id="152" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="tilt" type="int8"/>
|
||||
<field name="pan" type="int8"/>
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="booz_cam" dir="cam_control">
|
||||
<doc>
|
||||
<description>Camera control for BOOZ board</description>
|
||||
</doc>
|
||||
<!-- depend require="booz_pwm|led" -->
|
||||
<header>
|
||||
<file name="booz_cam.h"/>
|
||||
</header>
|
||||
<init fun="booz_cam_init()"/>
|
||||
<periodic fun="booz_cam_periodic()" freq="10."/>
|
||||
<datalink message="BOOZ_CAM_STICK" fun="BOOZ_CAM_STICK_PARSE(dl_buffer)"/>
|
||||
<makefile>
|
||||
<define name="USE_CAM"/>
|
||||
<file name="booz_cam.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="rotorcraft_cam" dir="cam_control">
|
||||
<doc>
|
||||
<description>
|
||||
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.
|
||||
</description>
|
||||
<section name="CAM" prefix="ROTORCRAFT_CAM_">
|
||||
<define name="DEFAULT_MODE" value="MODE_NONE|MODE_MANUAL|MODE_HEADING|MODE_WP" description="Select default mode (default: MODE_NONE)"/>
|
||||
<define name="CAM_ON" value="handler" description="Handler to power on the camera ({} to disable)"/>
|
||||
<define name="CAM_OFF" value="handler" description="Handler to power off the camera ({} to disable)"/>
|
||||
<define name="TILT_SERVO" value="name" description="Servo name for tilt control (no tilt control if not defined)"/>
|
||||
<define name="TILT_ANGLE_MIN" value="angle" description="Tilt angle corresponding to the servo min position"/>
|
||||
<define name="TILT_ANGLE_MAX" value="angle" description="Tilt angle corresponding to the servo max position"/>
|
||||
<define name="USE_PAN" value="TRUE|FALSE" description="Enable pan control with heading (default: TRUE)"/>
|
||||
<define name="TRACK_WP" value="WP_XXX" description="Enable waypoint tracking mode on the selected WP (default: enable if waypoint CAM is defined)"/>
|
||||
<define name="STICK_TILT_INC" value="angle/stick_input" description="Sensitivity of the datalink stick tilt control (default: 10 deg)"/>
|
||||
<define name="STICK_PAN_INC" value="angle/stick_input" description="Sensitivity of the datalink stick pan control (default: 20 deg)"/>
|
||||
</section>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="rotorcraft_cam.h"/>
|
||||
</header>
|
||||
<init fun="rotorcraft_cam_init()"/>
|
||||
<periodic fun="rotorcraft_cam_periodic()" freq="10."/>
|
||||
<datalink message="ROTORCRAFT_CAM_STICK" fun="ROTORCRAFT_CAM_STICK_PARSE(dl_buffer)"/>
|
||||
<makefile>
|
||||
<define name="USE_CAM"/>
|
||||
<file name="rotorcraft_cam.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<dl_settings>
|
||||
|
||||
<dl_settings NAME="CAM">
|
||||
<dl_setting var="booz_cam_mode" MIN="0" STEP="1" MAX="3" module="cam_control/booz_cam" shortname="mode" values="NONE|MAN|HEADING|WP" handler="SetCamMode">
|
||||
<dl_setting var="rotorcraft_cam_mode" MIN="0" STEP="1" MAX="3" module="cam_control/rotorcraft_cam" shortname="mode" values="NONE|MAN|HEADING|WP" handler="SetCamMode">
|
||||
<strip_button name="CN" value="0" group="cam_mode1"/>
|
||||
<strip_button name="CM" value="1" group="cam_mode1"/>
|
||||
<strip_button name="CH" value="2" group="cam_mode2"/>
|
||||
@@ -10,12 +10,12 @@
|
||||
<key_press key="F1" value="0"/>
|
||||
<key_press key="F2" value="1"/>
|
||||
</dl_setting>
|
||||
<dl_setting var="booz_cam_tilt_pwm" min="1000" step="1" max="2500" shortname="tilt_pwm"/>
|
||||
<dl_setting var="booz_cam_tilt" min="-90" step="1" max="0" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
|
||||
<dl_setting var="rotorcraft_cam_tilt_pwm" min="1000" step="1" max="2500" shortname="tilt_pwm"/>
|
||||
<dl_setting var="rotorcraft_cam_tilt" min="-90" step="1" max="0" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
|
||||
<strip_button name="Look Foreward" icon="lookfore.png" value="0" group="cam_look"/>
|
||||
<strip_button name="Look Down" icon="lookdown.png" value="-6434" group="cam_look"/>
|
||||
</dl_setting>
|
||||
<dl_setting var="booz_cam_pan" MIN="0" STEP="1" MAX="360" shortname="pan" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
|
||||
<dl_setting var="rotorcraft_cam_pan" MIN="0" STEP="1" MAX="360" shortname="pan" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
@@ -13,7 +13,7 @@
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
|
||||
<message name="WP_MOVED" period="1.3"/>
|
||||
<message name="BOOZ2_CAM" period="1."/>
|
||||
<message name="ROTORCRAFT_CAM" period="1."/>
|
||||
<message name="GPS_INT" period=".25"/>
|
||||
<message name="INS" period=".25"/>
|
||||
<message name="I2C_ERRORS" period="6."/>
|
||||
|
||||
@@ -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) { \
|
||||
|
||||
@@ -1,149 +0,0 @@
|
||||
/*
|
||||
* $Id: $
|
||||
*
|
||||
* Copyright (C) 2009 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
|
||||
* Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* 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
|
||||
}
|
||||
|
||||
@@ -1,73 +0,0 @@
|
||||
/*
|
||||
* $Id: $
|
||||
*
|
||||
* Copyright (C) 2009 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
|
||||
* Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* 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 */
|
||||
|
||||
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
* Copyright (C) 2009-2012 Gautier Hattenberger <gautier.hattenberger@enac.fr>,
|
||||
* Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* 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
|
||||
}
|
||||
|
||||
@@ -0,0 +1,139 @@
|
||||
/*
|
||||
* Copyright (C) 2009-2012 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
|
||||
* Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* 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 */
|
||||
|
||||
@@ -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 */
|
||||
|
||||
|
||||
Reference in New Issue
Block a user