Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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"
35 #include "subsystems/electrical.h"
38 
39 #if USE_GPS
40 #include "subsystems/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;
126 #else
127  int32_t state_psi = stateGetNedToBodyEulers_i()->psi;
128 #endif
129  pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
130  &(stateGetPositionEnu_i()->x),
131  &(stateGetPositionEnu_i()->y),
132  &(stateGetPositionEnu_i()->z),
133  &(stateGetSpeedEnu_i()->x),
134  &(stateGetSpeedEnu_i()->y),
135  &(stateGetSpeedEnu_i()->z),
136  &(stateGetNedToBodyEulers_i()->phi),
137  &(stateGetNedToBodyEulers_i()->theta),
138  &state_psi,
139  &guidance_h.sp.pos.y,
140  &guidance_h.sp.pos.x,
141  &carrot_up,
142  &carrot_heading,
143  &thrust,
145 }
146 
147 static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
148 {
149 #if USE_GPS
150  uint16_t gspeed = gps.gspeed;
151 #else
152  // ground speed in cm/s
153  uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100;
154 #endif
155  pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID,
156  &(stateGetPositionEnu_i()->x),
157  &(stateGetPositionEnu_i()->y),
158  &(stateGetPositionEnu_i()->z),
159  &gspeed);
160 }
161 
162 #ifdef RADIO_CONTROL
163 static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
164 {
165 #ifdef RADIO_KILL_SWITCH
167 #else
168  int16_t _kill_switch = 42;
169 #endif
170  pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
176  &_kill_switch,
178 }
179 #endif
180 
181 static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
182 {
183  pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
184  &stabilization_cmd[COMMAND_ROLL],
185  &stabilization_cmd[COMMAND_PITCH],
186  &stabilization_cmd[COMMAND_YAW],
187  &stabilization_cmd[COMMAND_THRUST]);
188 }
189 
190 
192 {
195 
196  // register messages
197  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
199  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
200  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
202 #ifdef RADIO_CONTROL
203  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
204 #endif
205 }
206 
211 void autopilot_event(void)
212 {
214 #ifdef AP_MODE_FAILSAFE
216 #endif
217  ) {
218  struct NedCoor_f *accel = stateGetAccelNed_f();
219  if (accel->z < -THRESHOLD_GROUND_DETECT ||
220  accel->z > THRESHOLD_GROUND_DETECT) {
221  autopilot.ground_detected = true;
223  }
224  }
225 }
226 
230 {
232 }
233 
236 void autopilot_check_in_flight(bool motors_on)
237 {
238  if (autopilot.in_flight) {
239  if (autopilot_in_flight_counter > 0) {
240  /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
241  if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
245  if (autopilot_in_flight_counter == 0) {
246  autopilot.in_flight = false;
247  }
248  } else { /* thrust, speed or accel not above min threshold, reset counter */
250  }
251  }
252  } else { /* currently not in flight */
254  motors_on) {
255  /* if thrust above min threshold, assume in_flight.
256  * Don't check for velocity and acceleration above threshold here...
257  */
258  if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
261  autopilot.in_flight = true;
262  }
263  } else { /* currently not in_flight and thrust below threshold, reset counter */
265  }
266  }
267  }
268 }
269 
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:129
unsigned short uint16_t
Definition: types.h:16
#define RADIO_ROLL
Definition: intermcu_ap.h:41
uint32_t nb_failure
Definition: motor_mixing.h:42
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
void autopilot_firmware_init(void)
Init function.
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:74
#define THRESHOLD_GROUND_DETECT
Z-acceleration threshold to detect ground in m/s^2.
static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry system header (includes downlink utility and generated code).
struct FloatEulers eulers_zxy
state eulers in zxy order
uint8_t status
Definition: radio_control.h:64
bool in_flight
in flight status
Definition: autopilot.h:70
static void send_status(struct transport_tx *trans, struct link_device *dev)
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
float psi
in radians
float vsupply
supply voltage in V
Definition: electrical.h:45
void autopilot_reset_in_flight_counter(void)
reset in_flight counter
uint32_t nb_saturation
Definition: motor_mixing.h:41
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
static void send_energy(struct transport_tx *trans, struct link_device *dev)
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
euler angles
float z
in meters
#define AUTOPILOT_IN_FLIGHT_TIME
time steps for in_flight detection (at 20Hz, so 20=1second)
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition: autopilot.h:75
#define RADIO_MODE
Definition: intermcu_ap.h:44
vector in North East Down coordinates Units: meters
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
Interface for electrical status: supply voltage, current, battery status, etc.
static void send_fp(struct transport_tx *trans, struct link_device *dev)
uint8_t autopilot_mode_auto2
float charge
consumed electric charge in Ah
Definition: electrical.h:47
Device independent GPS code (interface)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
unsigned long uint32_t
Definition: types.h:18
#define MODE_AUTO2
signed short int16_t
Definition: types.h:17
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define AUTOPILOT_IN_FLIGHT_MIN_SPEED
minimum vertical speed for in_flight condition in m/s
struct RadioControl radio_control
Definition: radio_control.c:30
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
uint8_t arming_status
arming status
Definition: autopilot.h:67
static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
void autopilot_event(void)
autopilot event function
uint8_t guidance_v_mode
Definition: guidance_v.c:103
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
bool motors_on
motor status
Definition: autopilot.h:68
unsigned char uint8_t
Definition: types.h:14
static uint32_t autopilot_in_flight_counter
#define RADIO_PITCH
Definition: intermcu_ap.h:42
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
#define RADIO_YAW
Definition: intermcu_ap.h:43
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
void autopilot_check_in_flight(bool motors_on)
in flight check utility function
struct MotorMixing motor_mixing
Definition: motor_mixing.c:94
#define AUTOPILOT_IN_FLIGHT_MIN_THRUST
minimum thrust for in_flight condition in pprz_t units (max = 9600)
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
float energy
consumed energy in Wh
Definition: electrical.h:48
struct Electrical electrical
Definition: electrical.c:66
#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL
minimum vertical acceleration for in_flight condition in m/s^2
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
Motor Mixing.
float current
current in A
Definition: electrical.h:46
bool ground_detected
automatic detection of landing
Definition: autopilot.h:74
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
#define AP_MODE_FAILSAFE
uint8_t fix
status of fix
Definition: gps.h:107
Rotorcraft specific autopilot interface and initialization.
struct GpsState gps
global GPS state
Definition: gps.c:69
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
uint8_t frame_rate
Definition: radio_control.h:67
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66