Paparazzi UAS  v5.15_devel-105-g9de12e2
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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(struct transport_tx *trans, struct link_device *dev)
92 {
93  pprz_msg_send_ROTORCRAFT_CAM(trans, dev, AC_ID,
95 }
96 
98 {
100 #ifdef ROTORCRAFT_CAM_SWITCH_GPIO
102  ROTORCRAFT_CAM_OFF(ROTORCRAFT_CAM_SWITCH_GPIO);
103  } else {
104  ROTORCRAFT_CAM_ON(ROTORCRAFT_CAM_SWITCH_GPIO);
105  }
106 #endif
107 }
108 
110 {
111 #ifdef ROTORCRAFT_CAM_SWITCH_GPIO
112  gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO);
113 #endif
115 #if ROTORCRAFT_CAM_USE_TILT
116  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
117  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
118 #else
120 #endif
122  rotorcraft_cam_pan = 0;
123 
124  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CAM, send_cam);
125 }
126 
128 {
129 
130  switch (rotorcraft_cam_mode) {
132 #if ROTORCRAFT_CAM_USE_TILT
133  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
134 #endif
135 #if ROTORCRAFT_CAM_USE_PAN
137 #endif
138  break;
140  // nothing to do here, just apply tilt pwm at the end
141  break;
143 #if ROTORCRAFT_CAM_USE_TILT_ANGLES
144  Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
145  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
146  (CAM_TA_MAX - CAM_TA_MIN);
147 #endif
148 #if ROTORCRAFT_CAM_USE_PAN
151 #endif
152  break;
154 #ifdef ROTORCRAFT_CAM_TRACK_WP
155  {
156  struct Int32Vect2 diff;
157  VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
158  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
159  rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
161 #if ROTORCRAFT_CAM_USE_TILT_ANGLES
162  int32_t dist, height;
163  dist = INT32_VECT2_NORM(diff);
164  height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
165  rotorcraft_cam_tilt = int32_atan2(height, dist);
166  Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
167  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
168  (CAM_TA_MAX - CAM_TA_MIN);
169 #endif
170  }
171 #endif
172  break;
173  default:
174  break;
175  }
176 #if ROTORCRAFT_CAM_USE_TILT
177  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
178 #endif
179 }
int32_t psi
in rad with INT32_ANGLE_FRAC
#define ROTORCRAFT_CAM_MODE_WP
Camera control module for rotorcraft.
Periodic telemetry system header (includes downlink utility and generated code).
int16_t rotorcraft_cam_tilt_pwm
#define ROTORCRAFT_CAM_MODE_MANUAL
static void send_cam(struct transport_tx *trans, struct link_device *dev)
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
int16_t rotorcraft_cam_pan
void rotorcraft_cam_periodic(void)
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
#define ROTORCRAFT_CAM_DEFAULT_MODE
Default mode is NONE.
#define ROTORCRAFT_CAM_MODE_HEADING
Hardware independent API for actuators (servos, motor controllers).
#define ROTORCRAFT_CAM_MODE_NONE
#define INT32_VECT2_NORM(_v)
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:104
signed short int16_t
Definition: types.h:17
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t z
Up.
#define INT32_POS_FRAC
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define ROTORCRAFT_CAM_ON
Gpio output to turn camera power power on.
Rotorcraft navigation functions.
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
API to get/set the generic vehicle states.
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
#define INT32_COURSE_NORMALIZE(_a)
uint8_t rotorcraft_cam_mode
WP control.
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
#define INT32_VECT2_RSHIFT(_o, _i, _r)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
void rotorcraft_cam_set_mode(uint8_t mode)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
int32_t int32_atan2(int32_t y, int32_t x)