Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
booz_cam.c
Go to the documentation of this file.
1 /*
2  * $Id: $
3  *
4  * Copyright (C) 2009 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
5  * Antoine Drouin <poinix@gmail.com>
6  *
7  * This file is part of paparazzi.
8  *
9  * paparazzi is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2, or (at your option)
12  * any later version.
13  *
14  * paparazzi is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with paparazzi; see the file COPYING. If not, write to
21  * the Free Software Foundation, 59 Temple Place - Suite 330,
22  * Boston, MA 02111-1307, USA.
23  */
24 
25 #include "cam_control/booz_cam.h"
26 #include "modules/core/booz_pwm_arch.h"
27 #include "subsystems/ahrs.h"
29 #include "subsystems/ins.h"
30 #include "generated/flight_plan.h"
31 #include "std.h"
32 
34 
35 // Tilt definition
36 #ifdef BOOZ_CAM_TILT_NEUTRAL
37 int16_t booz_cam_tilt_pwm;
38 int16_t booz_cam_tilt;
39 #ifndef BOOZ_CAM_TILT_MIN
40 #define BOOZ_CAM_TILT_MIN BOOZ_CAM_TILT_NEUTRAL
41 #endif
42 #ifndef BOOZ_CAM_TILT_MAX
43 #define BOOZ_CAM_TILT_MAX BOOZ_CAM_TILT_NEUTRAL
44 #endif
45 #define BOOZ_CAM_USE_TILT 1
46 #endif
47 
48 // Pan definition
49 #ifdef BOOZ_CAM_PAN_NEUTRAL
50 int16_t booz_cam_pan;
51 #ifndef BOOZ_CAM_PAN_MIN
52 #define BOOZ_CAM_PAN_MIN BOOZ_CAM_PAN_NEUTRAL
53 #endif
54 #ifndef BOOZ_CAM_PAN_MAX
55 #define BOOZ_CAM_PAN_MAX BOOZ_CAM_PAN_NEUTRAL
56 #endif
57 #define BOOZ_CAM_USE_PAN 1
58 #endif
59 
60 #if defined BOOZ_CAM_TILT_ANGLE_MIN && defined BOOZ_CAM_TILT_ANGLE_MAX && defined BOOZ_CAM_USE_TILT
61 #define CAM_TA_MIN ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MIN)
62 #define CAM_TA_MAX ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MAX)
63 #define BOOZ_CAM_USE_TILT_ANGLES 1
64 #endif
65 
66 // PWM definition
67 #ifndef BOOZ_CAM_SetPwm
68 #define BOOZ_CAM_SetPwm(_v) BoozSetPwmValue(_v)
69 #endif
70 
71 #ifndef BOOZ_CAM_DEFAULT_MODE
72 #define BOOZ_CAM_DEFAULT_MODE BOOZ_CAM_MODE_NONE
73 #endif
74 
75 void booz_cam_init(void) {
77 #ifdef BOOZ_CAM_USE_TILT
78  booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
79  BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
80  booz_cam_tilt = 0;
81 #endif
82 #ifdef BOOZ_CAM_USE_PAN
83  booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL;
84 #endif
85 }
86 
87 #define D_TILT (BOOZ_CAM_TILT_MAX - BOOZ_CAM_TILT_MIN)
88 #define CT_MIN Min(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX)
89 #define CT_MAX Max(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX)
90 #define CP_MIN Min(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX)
91 #define CP_MAX Max(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX)
92 
93 void booz_cam_periodic(void) {
94 
95  switch (booz_cam_mode) {
96  case BOOZ_CAM_MODE_NONE:
97 #ifdef BOOZ_CAM_USE_TILT
98  booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
99 #endif
100 #ifdef BOOZ_CAM_USE_PAN
101  booz_cam_pan = ahrs.ltp_to_body_euler.psi;
102 #endif
103  break;
105 #ifdef BOOZ_CAM_USE_TILT
106  Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
107 #endif
108  break;
110 #ifdef BOOZ_CAM_USE_TILT_ANGLES
111  Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX);
112  booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
113  Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
114 #endif
115 #ifdef BOOZ_CAM_USE_PAN
116  Bound(booz_cam_pan, CP_MIN, CP_MAX);
117  nav_heading = booz_cam_pan;
118 #endif
119  break;
120  case BOOZ_CAM_MODE_WP:
121 #ifdef WP_CAM
122  {
123  struct Int32Vect2 diff;
124  VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos);
126  INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
127  nav_heading = booz_cam_pan;
128 #ifdef BOOZ_CAM_USE_TILT_ANGLES
129  int32_t dist, height;
130  INT32_VECT2_NORM(dist, diff);
131  height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC;
132  INT32_ATAN2(booz_cam_tilt, height, dist);
133  Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
134  booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
135  Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
136 #endif
137  }
138 #endif
139  break;
140  }
141 #ifdef BOOZ_CAM_USE_TILT
142  BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
143 #endif
144 }
145 
#define CT_MAX
Definition: booz_cam.c:89
#define booz_cam_SetCamMode(_v)
Definition: booz_cam.h:61
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Attitude and Heading Reference System interface.
#define BOOZ_CAM_MODE_WP
Definition: booz_cam.h:36
#define INT32_POS_FRAC
struct EnuCoor_i ins_enu_pos
Definition: ins.c:80
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
Device independent INS code.
#define BOOZ_CAM_SetPwm(_v)
Definition: booz_cam.c:68
void booz_cam_init(void)
Definition: booz_cam.c:75
#define BOOZ_CAM_MODE_HEADING
Definition: booz_cam.h:35
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:69
#define BOOZ_CAM_MODE_MANUAL
Definition: booz_cam.h:34
#define INT32_ATAN2(_a, _y, _x)
signed short int16_t
Definition: types.h:17
#define CT_MIN
Definition: booz_cam.c:88
signed long int32_t
Definition: types.h:19
#define INT32_VECT2_NORM(n, v)
uint8_t booz_cam_mode
Definition: booz_cam.c:33
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
unsigned char uint8_t
Definition: types.h:14
#define CP_MAX
Definition: booz_cam.c:91
#define CP_MIN
Definition: booz_cam.c:90
int32_t psi
in rad with INT32_ANGLE_FRAC
#define BOOZ_CAM_MODE_NONE
Definition: booz_cam.h:33
#define D_TILT
Definition: booz_cam.c:87
#define BOOZ_CAM_DEFAULT_MODE
Definition: booz_cam.c:72
void booz_cam_periodic(void)
Definition: booz_cam.c:93