Paparazzi UAS  v5.15_devel-112-g521f3cf
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  pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
123  &(stateGetPositionEnu_i()->x),
124  &(stateGetPositionEnu_i()->y),
125  &(stateGetPositionEnu_i()->z),
126  &(stateGetSpeedEnu_i()->x),
127  &(stateGetSpeedEnu_i()->y),
128  &(stateGetSpeedEnu_i()->z),
132  &guidance_h.sp.pos.y,
133  &guidance_h.sp.pos.x,
134  &carrot_up,
135  &carrot_heading,
136  &thrust,
138 }
139 
140 static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
141 {
142 #if USE_GPS
143  uint16_t gspeed = gps.gspeed;
144 #else
145  // ground speed in cm/s
146  uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100;
147 #endif
148  pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID,
149  &(stateGetPositionEnu_i()->x),
150  &(stateGetPositionEnu_i()->y),
151  &(stateGetPositionEnu_i()->z),
152  &gspeed);
153 }
154 
155 #ifdef RADIO_CONTROL
156 static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
157 {
158 #ifdef RADIO_KILL_SWITCH
160 #else
161  int16_t _kill_switch = 42;
162 #endif
163  pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
169  &_kill_switch,
171 }
172 #endif
173 
174 static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
175 {
176  pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
177  &stabilization_cmd[COMMAND_ROLL],
178  &stabilization_cmd[COMMAND_PITCH],
179  &stabilization_cmd[COMMAND_YAW],
180  &stabilization_cmd[COMMAND_THRUST]);
181 }
182 
183 
185 {
188 
189  // register messages
190  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
192  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
193  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
195 #ifdef RADIO_CONTROL
196  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
197 #endif
198 }
199 
204 void autopilot_event(void)
205 {
207 #ifdef AP_MODE_FAILSAFE
209 #endif
210  ) {
211  struct NedCoor_f *accel = stateGetAccelNed_f();
212  if (accel->z < -THRESHOLD_GROUND_DETECT ||
213  accel->z > THRESHOLD_GROUND_DETECT) {
214  autopilot.ground_detected = true;
216  }
217  }
218 }
219 
223 {
225 }
226 
229 void autopilot_check_in_flight(bool motors_on)
230 {
231  if (autopilot.in_flight) {
232  if (autopilot_in_flight_counter > 0) {
233  /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
234  if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
238  if (autopilot_in_flight_counter == 0) {
239  autopilot.in_flight = false;
240  }
241  } else { /* thrust, speed or accel not above min threshold, reset counter */
243  }
244  }
245  } else { /* currently not in flight */
247  motors_on) {
248  /* if thrust above min threshold, assume in_flight.
249  * Don't check for velocity and acceleration above threshold here...
250  */
251  if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
254  autopilot.in_flight = true;
255  }
256  } else { /* currently not in_flight and thrust below threshold, reset counter */
258  }
259  }
260  }
261 }
262 
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
int32_t y
North.
int32_t x
East.
#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).
int32_t theta
in rad with INT32_ANGLE_FRAC
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 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)
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:80
int32_t z
Up.
#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:73
void autopilot_event(void)
autopilot event function
uint8_t guidance_v_mode
Definition: guidance_v.c:103
int32_t phi
in rad with INT32_ANGLE_FRAC
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