-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
rotorcraft_cam.c
174 lines (157 loc) · 5.86 KB
/
rotorcraft_cam.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
/*
* 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.
*/
/**
* @file modules/cam_control/rotorcraft_cam.c
* 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)
*
* If ROTORCRAFT_CAM_SWITCH_GPIO is defined, this gpio is set/cleared to switch the power
* of the camera on in normal modes and disable it when in NONE mode.
* On boards with CAM_SWITCH, ROTORCRAFT_CAM_SWITCH_GPIO can be defined to CAM_SWITCH_GPIO.
*/
#include "modules/cam_control/rotorcraft_cam.h"
#include "subsystems/actuators.h"
#include "state.h"
#include "firmwares/rotorcraft/navigation.h"
#include "std.h"
#include "subsystems/datalink/telemetry.h"
/** Gpio output to turn camera power power on.
* Control whether to set or clear the ROTORCRAFT_CAM_SWITCH_GPIO to turn on the camera power.
* Should be defined to either gpio_set (default) or gpio_clear.
* Not used if ROTORCRAFT_CAM_SWITCH_GPIO is not defined.
*/
#ifndef ROTORCRAFT_CAM_ON
#define ROTORCRAFT_CAM_ON gpio_set
#endif
/** Gpio output to turn camera power power off.
* Control whether to set or clear the ROTORCRAFT_CAM_SWITCH_GPIO to turn off the camera power.
* Should be defined to either gpio_set or gpio_clear (default).
* Not used if ROTORCRAFT_CAM_SWITCH_GPIO is not defined.
*/
#ifndef ROTORCRAFT_CAM_OFF
#define ROTORCRAFT_CAM_OFF gpio_clear
#endif
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
static void send_cam(void) {
DOWNLINK_SEND_ROTORCRAFT_CAM(DefaultChannel, DefaultDevice,
&rotorcraft_cam_tilt,&rotorcraft_cam_pan);
}
void rotorcraft_cam_set_mode(uint8_t mode) {
rotorcraft_cam_mode = mode;
#ifdef ROTORCRAFT_CAM_SWITCH_GPIO
if (rotorcraft_cam_mode == ROTORCRAFT_CAM_MODE_NONE) {
ROTORCRAFT_CAM_OFF(ROTORCRAFT_CAM_SWITCH_GPIO);
}
else {
ROTORCRAFT_CAM_ON(ROTORCRAFT_CAM_SWITCH_GPIO);
}
#endif
}
void rotorcraft_cam_init(void) {
#ifdef ROTORCRAFT_CAM_SWITCH_GPIO
gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO);
#endif
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;
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CAM", send_cam);
}
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);
rotorcraft_cam_pan = int32_atan2(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;
rotorcraft_cam_tilt = int32_atan2(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;
default:
break;
}
#if ROTORCRAFT_CAM_USE_TILT
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#endif
}