Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
hackhd.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
42 #include "generated/modules.h"
43 #include "generated/airframe.h"
44 #include "mcu_periph/gpio.h"
45 #include "mcu_periph/sys_time.h"
46 
48 #define HACKHD_PUSH gpio_clear
49 #define HACKHD_RELEASE gpio_set
50 
51 #ifndef HACKHD_GPIO
52 #error HACKHD: Please specify at least a HACKHD_GPIO (e.g. <define name="HACKHD_GPIO" value="GPIOC,GPIO5"/>)
53 #endif
54 
55 static inline uint32_t port_of_gpio(uint32_t port, uint16_t __attribute__((unused)) pin) { return port; }
56 static inline uint16_t pin_of_gpio(uint32_t __attribute__((unused)) port, uint16_t pin) { return pin; }
57 
59 #define HACKHD_POWER_DELAY 5.
60 
62 #define HACKHD_RECORD_DELAY 0.2
63 
67 #define HACKHD_LOG_DELAY 1000
68 
72 #define HACKHD_TIMER_OF_DELAY(_delay) ((uint32_t)(_delay * HACKHD_PERIODIC_FREQ))
73 
77 #ifndef HACKHD_AUTOSHOOT_DELAY
78 #define HACKHD_AUTOSHOOT_DELAY 4.0
79 #endif
80 #define HACKHD_AUTOSHOOT_TIMER_OF_DELAY(_delay) ((uint32_t)(_delay * HACKHD_AUTOSHOOT_FREQ))
81 
83 #if HACKHD_SYNC_SEND
84 
85 #include "mcu_periph/uart.h"
86 #include "pprzlink/messages.h"
88 #include "state.h"
89 #include "modules/gps/gps.h"
90 
91 static inline void hackhd_send_shot_position(void)
92 {
93  // angles in decideg
94  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
95  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
96  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
97  // course in decideg
98  int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
99  // ground speed in cm/s
101 
102  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
103  &hackhd.photo_nr,
104  &stateGetPositionLla_i()->lat,
105  &stateGetPositionLla_i()->lon,
106  &stateGetPositionLla_i()->alt,
107  &gps.hmsl,
108  &phi,
109  &theta,
110  &psi,
111  &course,
112  &speed,
113  &gps.tow);
114 }
115 #endif
116 
117 #if HACKHD_LOG
119 #include "state.h"
120 #include "modules/gps/gps.h"
121 
122 static inline void hackhd_log_shot_position(void)
123 {
124  // For unknown reason, the first shot is not taken
125  // so we start logging at photo_nr = 1
126  if (pprzLogFile != -1 && hackhd.photo_nr > 0) {
127  struct FloatEulers att = *stateGetNedToBodyEulers_f();
128  struct EnuCoor_f pos = *stateGetPositionEnu_f();
129  uint32_t time = get_sys_time_msec();
130  sdLogWriteLog(pprzLogFile, "%d %d %d %d %d %d %d %u\n",
132  (int32_t)(DegOfRad(att.phi * 10.0f)),
133  (int32_t)(DegOfRad(att.theta * 10.0f)),
134  (int32_t)(DegOfRad(att.psi * 10.0f)),
135  (int32_t)(pos.x * 100.0f),
136  (int32_t)(pos.y * 100.0f),
137  (int32_t)(pos.z * 100.0f),
138  time);
139  }
140 }
141 #endif
142 
143 struct HackHD hackhd;
144 
145 void hackhd_init(void)
146 {
148  hackhd.timer = 0;
149  hackhd.photo_nr = 0;
150  hackhd.autoshoot = 0;
151  hackhd.log_delay = 0;
152 
153 #ifndef SITL
154  gpio_setup_output(HACKHD_GPIO);
155  // set gpio as open-drain, only possible on stm32f4
156  gpio_set_output_options(
157  port_of_gpio(HACKHD_GPIO),
158  GPIO_OTYPE_OD,
159  GPIO_OSPEED_25MHZ,
160  pin_of_gpio(HACKHD_GPIO));
161  HACKHD_RELEASE(HACKHD_GPIO);
162 #endif
163 }
164 
165 void hackhd_periodic(void)
166 {
167  if (hackhd.timer) {
168  hackhd.timer--;
169  } else {
170  HACKHD_RELEASE(HACKHD_GPIO);
171  }
172  // test log delay if set
173  if (hackhd.log_delay) {
174 #ifndef SITL
176 #endif
177 #if HACKHD_LOG
178  hackhd_log_shot_position();
179 #endif
180 #if HACKHD_SYNC_SEND
181  hackhd_send_shot_position();
182 #endif
183  // increment photo
184  hackhd.photo_nr++;
185  // unset log delay
186  hackhd.log_delay = 0;
187 #ifndef SITL
188  }
189 #endif
190  }
191 }
192 
193 /* Command the powering and recording */
195 {
196  hackhd.status = cmd;
197  switch (cmd) {
198  case HACKHD_POWER_ON:
199  case HACKHD_POWER_OFF:
201  HACKHD_PUSH(HACKHD_GPIO);
202  break;
203  case HACKHD_START_RECORD:
204  case HACKHD_STOP_RECORD:
206  HACKHD_PUSH(HACKHD_GPIO);
207  break;
208  case HACKHD_SHOOT:
211  HACKHD_PUSH(HACKHD_GPIO);
214  break;
215  default:
216  break;
217  }
218 }
219 
221 {
222 // at least wait a minimum time before two shoots
223  if (hackhd.autoshoot) {
224  hackhd.autoshoot--;
225  } else {
226  // test distance if needed
227  // or take picture if first of the sequence
228 #ifdef HACKHD_AUTOSHOOT_DIST
229  struct EnuCoor_f pos = *stateGetPositionEnu_f();
230  struct FloatVect2 d_pos;
231  d_pos.x = pos.x - hackhd.last_shot_pos.x;
232  d_pos.y = pos.y - hackhd.last_shot_pos.y;
233  if (VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST * HACKHD_AUTOSHOOT_DIST)
235 #endif
236  // take a picture
238  // reset timer
240 #ifdef HACKHD_AUTOSHOOT_DIST
241  }
242 #endif
243  }
244 }
245 
247 {
248  // start taking a picture immediately
249  hackhd.autoshoot = 0;
251 }
252 
static int16_t course[3]
Definition: airspeed_uADC.c:58
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
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
Some architecture independent helper functions for GPIOs.
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
uint32_t tow
GPS time of week in ms.
Definition: gps.h:107
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
float phi
in radians
float theta
in radians
float psi
in radians
euler angles
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
#define HACKHD_RELEASE
Definition: hackhd.c:49
#define HACKHD_TIMER_OF_DELAY(_delay)
get timer from delay based on periodic freq from modules.h
Definition: hackhd.c:72
#define HACKHD_AUTOSHOOT_DELAY
autoshoot timer delay based on periodic freq from modules.h
Definition: hackhd.c:78
static uint32_t port_of_gpio(uint32_t port, uint16_t pin)
Definition: hackhd.c:55
void hackhd_autoshoot_start(void)
Definition: hackhd.c:246
#define HACKHD_PUSH
Trigger button is active low.
Definition: hackhd.c:48
struct HackHD hackhd
send report
Definition: hackhd.c:143
void hackhd_autoshoot(void)
Definition: hackhd.c:220
static uint16_t pin_of_gpio(uint32_t port, uint16_t pin)
Definition: hackhd.c:56
void hackhd_init(void)
Definition: hackhd.c:145
#define HACKHD_LOG_DELAY
delay in milli-seconds before logging after a shot this has been estimated to 1s
Definition: hackhd.c:67
void hackhd_periodic(void)
Definition: hackhd.c:165
#define HACKHD_POWER_DELAY
time in seconds to press the button to power on/off
Definition: hackhd.c:59
#define HACKHD_RECORD_DELAY
time in seconds to start/stop recording or take a picture
Definition: hackhd.c:62
#define HACKHD_AUTOSHOOT_TIMER_OF_DELAY(_delay)
Definition: hackhd.c:80
void hackhd_command(enum hackhd_status cmd)
Definition: hackhd.c:194
Digital video/photo recorder HackHD control.
enum hackhd_status status
Definition: hackhd.h:59
uint32_t timer
Definition: hackhd.h:60
uint32_t autoshoot
Definition: hackhd.h:62
uint32_t log_delay
Definition: hackhd.h:64
int16_t photo_nr
Definition: hackhd.h:61
hackhd_status
Definition: hackhd.h:48
@ HACKHD_STOP_RECORD
Definition: hackhd.h:53
@ HACKHD_START_RECORD
Definition: hackhd.h:52
@ HACKHD_POWER_ON
Definition: hackhd.h:50
@ HACKHD_SHOOT
Definition: hackhd.h:54
@ HACKHD_NONE
Definition: hackhd.h:49
@ HACKHD_AUTOSHOOT_START
Definition: hackhd.h:55
@ HACKHD_POWER_OFF
Definition: hackhd.h:51
struct EnuCoor_f last_shot_pos
Definition: hackhd.h:63
Definition: hackhd.h:58
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
API to get/set the generic vehicle states.
Architecture independent timing functions.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93