Paparazzi UAS  v6.3_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 
80 
81 #if USE_MOTOR_MIXING
83 #endif
84 
85 static void send_status(struct transport_tx *trans, struct link_device *dev)
86 {
87  uint32_t imu_nb_err = 0;
88 #if USE_MOTOR_MIXING
90 #else
91  uint8_t _motor_nb_err = 0;
92 #endif
93 #if USE_GPS
94  uint8_t fix = gps.fix;
95 #else
96  uint8_t fix = 0;
97 #endif
98  uint8_t in_flight = autopilot.in_flight;
99  uint8_t motors_on = autopilot.motors_on;
100  uint16_t time_sec = sys_time.nb_sec;
101  pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID,
102  &imu_nb_err, &_motor_nb_err,
104  &fix, &autopilot.mode, &in_flight, &motors_on,
106  &time_sec, &electrical.vsupply);
107 }
108 
109 static void send_energy(struct transport_tx *trans, struct link_device *dev)
110 {
111  uint8_t throttle = 100 * autopilot.throttle / MAX_PPRZ;
112  float power = electrical.vsupply * electrical.current;
113  pprz_msg_send_ENERGY(trans, dev, AC_ID,
115 }
116 
117 static void send_fp(struct transport_tx *trans, struct link_device *dev)
118 {
119  int32_t carrot_up = -guidance_v_z_sp;
120  int32_t carrot_heading = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
121  int32_t thrust = (int32_t)autopilot.throttle;
122 #if GUIDANCE_INDI_HYBRID
123  struct FloatEulers eulers_zxy;
125  struct Int32Eulers att;
127 #else
128  struct Int32Eulers att = *stateGetNedToBodyEulers_i();
129 #endif
130  pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
131  &(stateGetPositionEnu_i()->x),
132  &(stateGetPositionEnu_i()->y),
133  &(stateGetPositionEnu_i()->z),
134  &(stateGetSpeedEnu_i()->x),
135  &(stateGetSpeedEnu_i()->y),
136  &(stateGetSpeedEnu_i()->z),
137  &att.phi,
138  &att.theta,
139  &att.psi,
140  &guidance_h.sp.pos.y,
141  &guidance_h.sp.pos.x,
142  &carrot_up,
143  &carrot_heading,
144  &thrust,
146 }
147 
148 static void send_body_rates_accel(struct transport_tx *trans, struct link_device *dev)
149 {
150  pprz_msg_send_BODY_RATES_ACCEL(trans, dev, AC_ID,
151  &(stateGetBodyRates_f()->p),
152  &(stateGetBodyRates_f()->q),
153  &(stateGetBodyRates_f()->r),
154  &(stateGetAccelBody_i()->x),
155  &(stateGetAccelBody_i()->y),
156  &(stateGetAccelBody_i()->z));
157 }
158 
159 static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
160 {
161 #if USE_GPS
162  uint16_t gspeed = gps.gspeed;
163 #else
164  // ground speed in cm/s
165  uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100;
166 #endif
167  pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID,
168  &(stateGetPositionEnu_i()->x),
169  &(stateGetPositionEnu_i()->y),
170  &(stateGetPositionEnu_i()->z),
171  &gspeed);
172 }
173 
174 #ifdef RADIO_CONTROL
175 static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
176 {
177 #ifdef RADIO_KILL_SWITCH
179 #else
180  int16_t _kill_switch = 42;
181 #endif
182  pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
188  &_kill_switch,
190 }
191 #endif
192 
193 static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
194 {
195  pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
196  &stabilization_cmd[COMMAND_ROLL],
197  &stabilization_cmd[COMMAND_PITCH],
198  &stabilization_cmd[COMMAND_YAW],
199  &stabilization_cmd[COMMAND_THRUST]);
200 }
201 
202 
204 {
207 
208  // register messages
209  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
211  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
212  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
214  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BODY_RATES_ACCEL, send_body_rates_accel);
215 #ifdef RADIO_CONTROL
216  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
217 #endif
218 }
219 
224 void autopilot_event(void)
225 {
227 #ifdef AP_MODE_FAILSAFE
229 #endif
230  ) {
231  struct NedCoor_f *accel = stateGetAccelNed_f();
232  if (accel->z < -THRESHOLD_GROUND_DETECT ||
233  accel->z > THRESHOLD_GROUND_DETECT) {
234  autopilot.ground_detected = true;
236  }
237  }
238 }
239 
243 {
245 }
246 
249 void autopilot_check_in_flight(bool motors_on)
250 {
251  if (autopilot.in_flight) {
252  if (autopilot_in_flight_counter > 0) {
253  /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
254  if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
258  if (autopilot_in_flight_counter == 0) {
259  autopilot.in_flight = false;
260  }
261  } else { /* thrust, speed or accel not above min threshold, reset counter */
263  }
264  }
265  } else { /* currently not in flight */
267  motors_on) {
268  /* if thrust above min threshold, assume in_flight.
269  * Don't check for velocity and acceleration above threshold here...
270  */
271  if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
274  autopilot.in_flight = true;
275  }
276  } else { /* currently not in_flight and thrust below threshold, reset counter */
278  }
279  }
280  }
281 }
282 
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
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
struct Electrical electrical
Definition: electrical.c:66
Interface for electrical status: supply voltage, current, battery status, etc.
float energy
consumed energy in Wh
Definition: electrical.h:48
float current
current in A
Definition: electrical.h:46
float charge
consumed electric charge in Ah
Definition: electrical.h:47
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:100
uint8_t fix
status of fix
Definition: gps.h:110
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
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.
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_fp(struct transport_tx *trans, struct link_device *dev)
static uint32_t autopilot_in_flight_counter
void autopilot_reset_in_flight_counter(void)
reset in_flight counter
static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
#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:90
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:74
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
uint8_t guidance_v_mode
Definition: guidance_v.c:103
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:129
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
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