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
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 "messages.h"
88 #include "state.h"
89 #include "subsystems/gps.h"
90 
91 static inline void hackhd_send_shot_position(void)
92 {
93  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
94  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
95  float gps_z = ((float)gps.hmsl) / 1000.0f;
96  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
97 
98  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
100  &gps.utm_pos.east,
101  &gps.utm_pos.north,
102  &gps_z,
103  &gps.utm_pos.zone,
104  &phi,
105  &theta,
106  &course,
107  &gps.gspeed,
108  &gps.tow);
109 }
110 #endif
111 
112 #if HACKHD_LOG
113 #include "sdLog.h"
114 #include "subsystems/chibios-libopencm3/chibios_sdlog.h"
115 #include "state.h"
116 #include "subsystems/gps.h"
117 #include "subsystems/ahrs.h"
118 
119 static inline void hackhd_log_shot_position(void)
120 {
121  // For unknown reason, the first shot is not taken
122  // so we start logging at photo_nr = 1
123  if (pprzLogFile.fs != NULL && hackhd.photo_nr > 0) {
124  struct FloatEulers att = *stateGetNedToBodyEulers_f();
125 #if INS_UPDATE_FW_ESTIMATOR
126  att.phi += ins_roll_neutral;
127  att.theta += ins_pitch_neutral;
128 #endif
129  struct EnuCoor_f pos = *stateGetPositionEnu_f();
130  uint32_t time = get_sys_time_msec();
131  sdLogWriteLog(&pprzLogFile, "%d %d %d %d %d %d %d %u\n",
133  (int32_t)(DegOfRad(att.phi*10.0f)),
134  (int32_t)(DegOfRad(att.theta*10.0f)),
135  (int32_t)(DegOfRad(att.psi*10.0f)),
136  (int32_t)(pos.x*100.0f),
137  (int32_t)(pos.y*100.0f),
138  (int32_t)(pos.z*100.0f),
139  time);
140  }
141 }
142 #endif
143 
144 struct HackHD hackhd;
145 
146 void hackhd_init(void) {
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  if (hackhd.timer) {
167  hackhd.timer--;
168  } else {
169  HACKHD_RELEASE(HACKHD_GPIO);
170  }
171  // test log delay if set
172  if (hackhd.log_delay) {
173 #ifndef SITL
175 #endif
176 #if HACKHD_LOG
177  hackhd_log_shot_position();
178 #endif
179 #if HACKHD_SYNC_SEND
180  hackhd_send_shot_position();
181 #endif
182  // increment photo
183  hackhd.photo_nr++;
184  // unset log delay
185  hackhd.log_delay = 0;
186 #ifndef SITL
187  }
188 #endif
189  }
190 }
191 
192 /* Command the powering and recording */
194  hackhd.status = cmd;
195  switch (cmd)
196  {
197  case HACKHD_POWER_ON:
198  case HACKHD_POWER_OFF:
200  HACKHD_PUSH(HACKHD_GPIO);
201  break;
202  case HACKHD_START_RECORD:
203  case HACKHD_STOP_RECORD:
205  HACKHD_PUSH(HACKHD_GPIO);
206  break;
207  case HACKHD_SHOOT:
210  HACKHD_PUSH(HACKHD_GPIO);
213  break;
214  default:
215  break;
216  }
217 }
218 
219 void hackhd_autoshoot(void) {
220  // at least wait a minimum time before two shoots
221  if (hackhd.autoshoot) {
222  hackhd.autoshoot--;
223  }
224  else {
225  // test distance if needed
226  // or take picture if first of the sequence
227 #ifdef HACKHD_AUTOSHOOT_DIST
228  struct EnuCoor_f pos = *stateGetPositionEnu_f();
229  struct FloatVect2 d_pos;
230  d_pos.x = pos.x - hackhd.last_shot_pos.x;
231  d_pos.y = pos.y - hackhd.last_shot_pos.y;
232  if (FLOAT_VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST*HACKHD_AUTOSHOOT_DIST)
234 #endif
235  // take a picture
237  // reset timer
239 #ifdef HACKHD_AUTOSHOOT_DIST
240  }
241 #endif
242  }
243 }
244 
246  // start taking a picture immediately
247  hackhd.autoshoot = 0;
249 }
250 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Attitude and Heading Reference System interface.
void hackhd_periodic(void)
Definition: hackhd.c:165
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
hackhd_status
Definition: hackhd.h:48
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
uint8_t zone
UTM zone number.
float psi
in radians
void hackhd_init(void)
Definition: hackhd.c:146
Some architecture independent helper functions for GPIOs.
void hackhd_autoshoot(void)
Definition: hackhd.c:219
float z
in meters
struct HackHD hackhd
send report
Definition: hackhd.c:144
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
float theta
in radians
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
euler angles
float y
in meters
struct EnuCoor_f last_shot_pos
Definition: hackhd.h:63
#define HACKHD_RECORD_DELAY
time in seconds to start/stop recording or take a picture
Definition: hackhd.c:62
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
#define HACKHD_POWER_DELAY
time in seconds to press the button to power on/off
Definition: hackhd.c:59
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
float phi
in radians
Architecture independent timing functions.
Device independent GPS code (interface)
void hackhd_command(enum hackhd_status cmd)
Definition: hackhd.c:193
static uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.h:75
uint32_t log_delay
Definition: hackhd.h:64
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
float x
in meters
static uint32_t port_of_gpio(uint32_t port, uint16_t pin)
Definition: hackhd.c:55
uint32_t timer
Definition: hackhd.h:60
int32_t north
in centimeters
#define FLOAT_VECT2_NORM2(_v)
#define HACKHD_AUTOSHOOT_TIMER_OF_DELAY(_delay)
Definition: hackhd.c:80
enum hackhd_status status
Definition: hackhd.h:59
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define HACKHD_LOG_DELAY
delay in milli-seconds before logging after a shot this has been estimated to 1s
Definition: hackhd.c:67
signed long int32_t
Definition: types.h:19
uint32_t photo_nr
Definition: hackhd.h:61
#define HACKHD_AUTOSHOOT_DELAY
autoshoot timer delay based on periodic freq from modules.h
Definition: hackhd.c:78
void hackhd_autoshoot_start(void)
Definition: hackhd.c:245
int32_t east
in centimeters
Digital video/photo recorder HackHD control.
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
uint32_t autoshoot
Definition: hackhd.h:62
#define HACKHD_TIMER_OF_DELAY(_delay)
get timer from delay based on periodic freq from modules.h
Definition: hackhd.c:72
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
vector in East North Up coordinates Units: meters
Definition: hackhd.h:58
float ins_roll_neutral
Definition: ins_arduimu.c:14
#define HACKHD_PUSH
Trigger button is active low.
Definition: hackhd.c:48
static uint16_t pin_of_gpio(uint32_t port, uint16_t pin)
Definition: hackhd.c:56
#define HACKHD_RELEASE
Definition: hackhd.c:49
struct GpsState gps
global GPS state
Definition: gps.c:41