Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
rotorcraft_cam.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009-2012 Gautier Hattenberger <gautier.hattenberger@enac.fr>,
3  * Antoine Drouin <poinix@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
42 
43 #include "subsystems/actuators.h"
44 #include "state.h"
46 #include "std.h"
47 
49 
50 
56 #ifndef ROTORCRAFT_CAM_ON
57 #define ROTORCRAFT_CAM_ON gpio_set
58 #endif
59 
65 #ifndef ROTORCRAFT_CAM_OFF
66 #define ROTORCRAFT_CAM_OFF gpio_clear
67 #endif
68 
70 
71 #define _SERVO_PARAM(_s,_p) SERVO_ ## _s ## _ ## _p
72 #define SERVO_PARAM(_s,_p) _SERVO_PARAM(_s,_p)
73 
74 // Tilt definition
77 #if ROTORCRAFT_CAM_USE_TILT
78 #define ROTORCRAFT_CAM_TILT_NEUTRAL SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, NEUTRAL)
79 #define ROTORCRAFT_CAM_TILT_MIN SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MIN)
80 #define ROTORCRAFT_CAM_TILT_MAX SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MAX)
81 #define D_TILT (ROTORCRAFT_CAM_TILT_MAX - ROTORCRAFT_CAM_TILT_MIN)
82 #define CT_MIN Min(CAM_TA_MIN, CAM_TA_MAX)
83 #define CT_MAX Max(CAM_TA_MIN, CAM_TA_MAX)
84 #endif
85 
86 // Pan definition
88 #define ROTORCRAFT_CAM_PAN_MIN 0
89 #define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI
90 
91 static void send_cam(void) {
92  DOWNLINK_SEND_ROTORCRAFT_CAM(DefaultChannel, DefaultDevice,
94 }
95 
97  rotorcraft_cam_mode = mode;
98 #ifdef ROTORCRAFT_CAM_SWITCH_GPIO
100  ROTORCRAFT_CAM_OFF(ROTORCRAFT_CAM_SWITCH_GPIO);
101  }
102  else {
103  ROTORCRAFT_CAM_ON(ROTORCRAFT_CAM_SWITCH_GPIO);
104  }
105 #endif
106 }
107 
109 #ifdef ROTORCRAFT_CAM_SWITCH_GPIO
110  gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO);
111 #endif
113 #if ROTORCRAFT_CAM_USE_TILT
114  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
115  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
116 #else
118 #endif
120  rotorcraft_cam_pan = 0;
121 
122  register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CAM", send_cam);
123 }
124 
126 
127  switch (rotorcraft_cam_mode) {
129 #if ROTORCRAFT_CAM_USE_TILT
130  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
131 #endif
132 #if ROTORCRAFT_CAM_USE_PAN
134 #endif
135  break;
137  // nothing to do here, just apply tilt pwm at the end
138  break;
140 #if ROTORCRAFT_CAM_USE_TILT_ANGLES
141  Bound(rotorcraft_cam_tilt,CT_MIN,CT_MAX);
142  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
143 #endif
144 #if ROTORCRAFT_CAM_USE_PAN
147 #endif
148  break;
150 #ifdef ROTORCRAFT_CAM_TRACK_WP
151  {
152  struct Int32Vect2 diff;
153  VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
155  INT32_ATAN2(rotorcraft_cam_pan,diff.x,diff.y);
157 #if ROTORCRAFT_CAM_USE_TILT_ANGLES
158  int32_t dist, height;
159  INT32_VECT2_NORM(dist, diff);
160  height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
161  INT32_ATAN2(rotorcraft_cam_tilt, height, dist);
162  Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
163  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
164 #endif
165  }
166 #endif
167  break;
168  default:
169  break;
170  }
171 #if ROTORCRAFT_CAM_USE_TILT
172  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
173 #endif
174 }
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define ROTORCRAFT_CAM_MODE_WP
Camera control module for rotorcraft.
Periodic telemetry system header (includes downlink utility and generated code).
#define INT32_POS_FRAC
int16_t rotorcraft_cam_tilt_pwm
#define ROTORCRAFT_CAM_MODE_MANUAL
int16_t rotorcraft_cam_pan
void rotorcraft_cam_periodic(void)
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
#define ROTORCRAFT_CAM_DEFAULT_MODE
Default mode is NONE.
#define ROTORCRAFT_CAM_MODE_HEADING
int32_t z
Up.
#define ROTORCRAFT_CAM_MODE_NONE
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
#define INT32_ATAN2(_a, _y, _x)
signed short int16_t
Definition: types.h:17
Hardware independent API for actuators (servos, motor controllers).
signed long int32_t
Definition: types.h:19
#define INT32_VECT2_NORM(n, v)
#define ROTORCRAFT_CAM_ON
Gpio output to turn camera power power on.
#define INT32_COURSE_NORMALIZE(_a)
int16_t rotorcraft_cam_tilt
#define ROTORCRAFT_CAM_OFF
Gpio output to turn camera power power off.
void rotorcraft_cam_init(void)
unsigned char uint8_t
Definition: types.h:14
#define rotorcraft_cam_SetCamMode(_v)
Set camera mode.
API to get/set the generic vehicle states.
static void gpio_setup_output(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.h:76
int32_t psi
in rad with INT32_ANGLE_FRAC
uint8_t rotorcraft_cam_mode
WP control.
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:637
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
void rotorcraft_cam_set_mode(uint8_t mode)
static void send_cam(void)