Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
autopilot_firmware.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2012 The Paparazzi Team
3  * Copyright (C) 2016-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
30 
31 #include "generated/modules.h"
32 
33 #include <stdint.h>
34 //#include "mcu_periph/sys_time.h"
38 
39 #if USE_GPS
40 #include "modules/gps/gps.h"
41 #else
42 #if NO_GPS_NEEDED_FOR_NAV
43 #define GpsIsLost() FALSE
44 #else
45 #define GpsIsLost() TRUE
46 #endif
47 #endif
48 
51 
52 /* Geofence exceptions */
54 
56 #ifndef AUTOPILOT_IN_FLIGHT_TIME
57 #define AUTOPILOT_IN_FLIGHT_TIME 20
58 #endif
59 
61 #ifndef AUTOPILOT_IN_FLIGHT_MIN_SPEED
62 #define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2
63 #endif
64 
66 #ifndef AUTOPILOT_IN_FLIGHT_MIN_ACCEL
67 #define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0
68 #endif
69 
71 #ifndef AUTOPILOT_IN_FLIGHT_MIN_THRUST
72 #define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
73 #endif
74 
76 #ifndef THRESHOLD_GROUND_DETECT
77 #define THRESHOLD_GROUND_DETECT 25.0
78 #endif
79 
81 bool WEAK autopilot_ground_detection(void) {
82  struct NedCoor_f *accel = stateGetAccelNed_f();
83  if (accel->z < -THRESHOLD_GROUND_DETECT ||
84  accel->z > THRESHOLD_GROUND_DETECT) {
85  return true;
86  }
87  return false;
88 }
89 
90 
92 bool WEAK autopilot_in_flight_end_detection(bool motors_on UNUSED) {
94  /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
95  if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
99  if (autopilot_in_flight_counter == 0) {
100  return true;
101  }
102  } else { /* thrust, speed or accel not above min threshold, reset counter */
104  }
105  }
106  return false;
107 }
108 
109 
110 #if USE_MOTOR_MIXING
112 #endif
113 
114 static void send_status(struct transport_tx *trans, struct link_device *dev)
115 {
116  uint32_t imu_nb_err = 0;
117 #if USE_MOTOR_MIXING
119 #else
120  uint8_t _motor_nb_err = 0;
121 #endif
122 #if USE_GPS
123  uint8_t fix = gps.fix;
124 #else
125  uint8_t fix = 0;
126 #endif
127  uint8_t in_flight = autopilot.in_flight;
128  uint8_t motors_on = autopilot.motors_on;
129  uint16_t time_sec = sys_time.nb_sec;
130  pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID,
131  &imu_nb_err, &_motor_nb_err,
133  &fix, &autopilot.mode, &in_flight, &motors_on,
135  &time_sec, &electrical.vsupply, &electrical.vboard);
136 }
137 
138 static void send_energy(struct transport_tx *trans, struct link_device *dev)
139 {
140  uint8_t throttle = 100 * autopilot.throttle / MAX_PPRZ;
141  float power = electrical.vsupply * electrical.current;
142  float avg_power = 0;
143  if(electrical.avg_cnt != 0) {
144  avg_power = (float)electrical.avg_power / electrical.avg_cnt;
145  }
146 
147  pprz_msg_send_ENERGY(trans, dev, AC_ID,
148  &throttle, &electrical.vsupply, &electrical.current, &power, &avg_power, &electrical.charge, &electrical.energy);
149 }
150 
151 static void send_fp(struct transport_tx *trans, struct link_device *dev)
152 {
153  int32_t carrot_up = -guidance_v.z_sp;
154  int32_t carrot_heading = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
155  int32_t thrust = (int32_t)autopilot.throttle;
156  struct EnuCoor_i *pos = stateGetPositionEnu_i();
157 #if GUIDANCE_INDI_HYBRID
158  struct FloatEulers eulers_zxy;
160  struct Int32Eulers att;
162 #else
163  struct Int32Eulers att = *stateGetNedToBodyEulers_i();
164 #endif
165  pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
166  &pos->x,
167  &pos->y,
168  &pos->z,
169  &(stateGetSpeedEnu_i()->x),
170  &(stateGetSpeedEnu_i()->y),
171  &(stateGetSpeedEnu_i()->z),
172  &att.phi,
173  &att.theta,
174  &att.psi,
175  &guidance_h.sp.pos.y,
176  &guidance_h.sp.pos.x,
177  &carrot_up,
178  &carrot_heading,
179  &thrust,
181 }
182 
183 static void send_body_rates_accel(struct transport_tx *trans, struct link_device *dev)
184 {
185  pprz_msg_send_BODY_RATES_ACCEL(trans, dev, AC_ID,
186  &(stateGetBodyRates_f()->p),
187  &(stateGetBodyRates_f()->q),
188  &(stateGetBodyRates_f()->r),
189  &(stateGetAccelBody_i()->x),
190  &(stateGetAccelBody_i()->y),
191  &(stateGetAccelBody_i()->z));
192 }
193 
194 static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
195 {
196 #if USE_GPS
197  uint16_t gspeed = gps.gspeed;
198 #else
199  // ground speed in cm/s
200  uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100;
201 #endif
202  pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID,
203  &(stateGetPositionEnu_i()->x),
204  &(stateGetPositionEnu_i()->y),
205  &(stateGetPositionEnu_i()->z),
206  &gspeed);
207 }
208 
209 #ifdef RADIO_CONTROL
210 static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
211 {
212 #ifdef RADIO_KILL_SWITCH
214 #else
215  int16_t _kill_switch = 42;
216 #endif
217  pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
223  &_kill_switch,
225 }
226 #endif
227 
228 #if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
229 static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
230 {
231  pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
232  &stabilization_cmd[COMMAND_ROLL],
233  &stabilization_cmd[COMMAND_PITCH],
234  &stabilization_cmd[COMMAND_YAW],
235  &stabilization_cmd[COMMAND_THRUST]);
236 }
237 #else
238 static void send_rotorcraft_cmd(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED) {}
239 #endif
240 
241 
243 {
246 
247  // register messages
248  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
250  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
251  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
253  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BODY_RATES_ACCEL, send_body_rates_accel);
254 #ifdef RADIO_CONTROL
255  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
256 #endif
257 }
258 
263 void autopilot_event(void)
264 {
266 #ifdef AP_MODE_FAILSAFE
268 #endif
269  ) {
271  autopilot.ground_detected = true;
273  }
274  }
275 }
276 
280 {
282 }
283 
286 void autopilot_check_in_flight(bool motors_on)
287 {
288  if (autopilot.in_flight) {
289  if (autopilot_in_flight_end_detection(motors_on)) {
290  autopilot.in_flight = false;
292  }
293  } else { /* currently not in flight */
295  motors_on) {
296  /* if thrust above min threshold, assume in_flight.
297  * Don't check for velocity and acceleration above threshold here...
298  */
299  if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
302  autopilot.in_flight = true;
303  }
304  } else { /* currently not in_flight and thrust below threshold, reset counter */
306  }
307  }
308  }
309 }
310 
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
bool motors_on
motor status
Definition: autopilot.h:68
bool ground_detected
automatic detection of landing
Definition: autopilot.h:73
uint8_t arming_status
arming status
Definition: autopilot.h:67
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition: autopilot.h:74
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
bool in_flight
in flight status
Definition: autopilot.h:70
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
uint8_t last_wp UNUSED
struct Electrical electrical
Definition: electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
uint32_t avg_power
average power sum
Definition: electrical.h:53
float energy
consumed energy in Wh
Definition: electrical.h:49
float current
current in A
Definition: electrical.h:47
uint32_t avg_cnt
average power counter
Definition: electrical.h:54
float charge
consumed electric charge in Ah
Definition: electrical.h:48
float vboard
board voltage in V
Definition: electrical.h:46
float vsupply
supply voltage in V
Definition: electrical.h:45
void autopilot_firmware_init(void)
Init function.
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
uint8_t fix
status of fix
Definition: gps.h:105
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
euler angles
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define ANGLE_BFP_OF_REAL(_af)
euler angles
vector in East North Up coordinates
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
struct FloatEulers eulers_zxy
state eulers in zxy order
static float p[2][2]
struct MotorMixing motor_mixing
Definition: motor_mixing.c:94
Motor Mixing.
uint32_t nb_failure
Definition: motor_mixing.h:42
uint32_t nb_saturation
Definition: motor_mixing.h:41
Optional exceptions triggeringg HOME_MODE 1) GEOFENCE_DATALINK_LOST_TIME: go to HOME mode if datalink...
#define MAX_PPRZ
Definition: paparazzi.h:8
float z
in meters
vector in North East Down coordinates Units: meters
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
uint8_t status
Definition: radio_control.h:61
uint8_t frame_rate
Definition: radio_control.h:64
#define RADIO_KILL_SWITCH
Definition: rc_intermcu.h:40
void autopilot_event(void)
autopilot event function
#define AUTOPILOT_IN_FLIGHT_MIN_SPEED
minimum vertical speed for in_flight condition in m/s
static void send_status(struct transport_tx *trans, struct link_device *dev)
uint8_t autopilot_mode_auto2
#define AUTOPILOT_IN_FLIGHT_TIME
time steps for in_flight detection (at 20Hz, so 20=1second)
static void send_energy(struct transport_tx *trans, struct link_device *dev)
static void send_body_rates_accel(struct transport_tx *trans, struct link_device *dev)
#define THRESHOLD_GROUND_DETECT
Z-acceleration threshold to detect ground in m/s^2.
bool WEAK autopilot_in_flight_end_detection(bool motors_on UNUSED)
Default end-of-in-flight detection estimation based on thrust and speed.
static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
void autopilot_check_in_flight(bool motors_on)
in flight check utility function
static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL
minimum vertical acceleration for in_flight condition in m/s^2
static void send_rotorcraft_cmd(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
static void send_fp(struct transport_tx *trans, struct link_device *dev)
bool WEAK autopilot_ground_detection(void)
Default ground-detection estimation based on accelerometer shock.
static uint32_t autopilot_in_flight_counter
void autopilot_reset_in_flight_counter(void)
reset in_flight counter
#define AUTOPILOT_IN_FLIGHT_MIN_THRUST
minimum thrust for in_flight condition in pprz_t units (max = 9600)
Rotorcraft specific autopilot interface and initialization.
#define MODE_AUTO2
#define AP_MODE_FAILSAFE
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:50
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:109
struct VerticalGuidance guidance_v
Definition: guidance_v.c:59
int32_t z_sp
altitude setpoint in meters (input).
Definition: guidance_v.h:51
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:34
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:80
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98