Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
autopilot_firmware.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
30 
31 #include "state.h"
33 #include <stdint.h>
35 
36 PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
37 
39 
40 #if PERIODIC_TELEMETRY
42 #include "generated/settings.h"
43 
44 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
45 #include "modules/settings/rc_settings.h"
46 static void send_rc_settings(struct transport_tx *trans, struct link_device *dev)
47 {
48  if (!RcSettingsOff()) {
49  pprz_msg_send_SETTINGS(trans, dev, AC_ID, &slider_1_val, &slider_2_val);
50  }
51 }
52 #else
54 #endif
55 
56 static void send_mode(struct transport_tx *trans, struct link_device *dev)
57 {
65  uint8_t mcu1_status = 0;
66 #ifdef RADIO_CONTROL
67  mcu1_status |= (radio_control.status == RC_OK ? (1<<0) : 0);
68  mcu1_status |= (radio_control.status == RC_REALLY_LOST ? (1<<2) : 0);
69  mcu1_status |= (radio_control.status == RC_OK ? (1<<4) : 0);
70 #endif
71 #ifdef AP_MODE_MANUAL
72  mcu1_status |= (autopilot_get_mode() == AP_MODE_MANUAL ? 0 : (1<<2));
73 #endif
74 
75  pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID,
77 }
78 
79 static void send_estimator(struct transport_tx *trans, struct link_device *dev)
80 {
81  pprz_msg_send_ESTIMATOR(trans, dev, AC_ID,
82  &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
83 }
84 
85 static void send_energy(struct transport_tx *trans, struct link_device *dev)
86 {
87  uint8_t throttle = 100 * autopilot.throttle / MAX_PPRZ;
88  float power = electrical.vsupply * electrical.current;
89  pprz_msg_send_ENERGY(trans, dev, AC_ID,
91 }
92 
93 // FIXME not the best place
95 #include CTRL_TYPE_H
96 static void send_desired(struct transport_tx *trans, struct link_device *dev)
97 {
98 #ifndef USE_AIRSPEED
99  float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
100 #endif
101  pprz_msg_send_DESIRED(trans, dev, AC_ID,
105 }
106 
107 static void send_airspeed(struct transport_tx *trans __attribute__((unused)),
108  struct link_device *dev __attribute__((unused)))
109 {
110  float airspeed = stateGetAirspeed_f();
111 #if USE_AIRSPEED
112  pprz_msg_send_AIRSPEED(trans, dev, AC_ID,
113  &airspeed, &v_ctl_auto_airspeed_setpoint,
115 #else
116  float zero = 0;
117  pprz_msg_send_AIRSPEED(trans, dev, AC_ID, &airspeed, &zero, &zero, &zero);
118 #endif
119 }
120 
121 #if !INTERMCU_AP
123 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
124 {
125  uint8_t fbw_mode = 1; // old FBW_AUTO
126 #ifdef AP_MODE_MANUAL
127  if (autopilot_get_mode() == AP_MODE_MANUAL) { fbw_mode = 0; } // FBW Manual
128 #endif
129 #ifdef RADIO_CONTROL
130  uint8_t rc_status = radio_control.status;
131  uint8_t rc_rate = radio_control.frame_rate;
132 #else
133  uint8_t rc_status = 0;
134  uint8_t rc_rate = 0;
135 #endif
136  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
137  &rc_status, &rc_rate, &fbw_mode,
139 }
140 #endif
141 
142 #endif /* PERIODIC_TELEMETRY */
143 
145 {
146  // use default telemetry here
147 #if DOWNLINK
148  send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
149 #endif
150 }
151 
153 {
154 #if PERIODIC_TELEMETRY
155  /* register some periodic message */
156  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PPRZ_MODE, send_mode);
161 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
162  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC_SETTINGS, send_rc_settings);
163 #endif
164 #if !INTERMCU_AP
166 #endif
167 #endif
168 }
169 
170 
171 #include "generated/flight_plan.h"
172 #include "generated/airframe.h"
173 
174 #ifdef LOW_BATTERY_KILL_DELAY
175 #warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file!
176 #endif
177 
179 #ifndef CATASTROPHIC_BAT_KILL_DELAY
180 #define CATASTROPHIC_BAT_KILL_DELAY 5
181 #endif
182 
184 #ifndef KILL_MODE_DISTANCE
185 #define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
186 #endif
187 
189 #ifndef MIN_SPEED_FOR_TAKEOFF
190 #define MIN_SPEED_FOR_TAKEOFF 5.
191 #endif
192 
194 void monitor_task(void)
195 {
196  static uint8_t t = 0;
197  if (electrical.vsupply < CATASTROPHIC_BAT_LEVEL) {
198  t++;
199  } else {
200  t = 0;
201  }
202 #if !USE_GENERATED_AUTOPILOT
203  // only check for static autopilot
206 #endif
207 
208  if (!autopilot.flight_time &&
211  autopilot.launch = true; /* Not set in non auto launch */
212 #if DOWNLINK
213  uint16_t time_sec = sys_time.nb_sec;
214  DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
215 #endif
216  }
217 
218 }
219 
220 
radio_control.h
electrical.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
v_ctl_mode
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
send_fbw_status
static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot_firmware.c:123
send_desired
static void send_desired(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot_firmware.c:96
dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:91
pprz_autopilot::flight_time
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
pprz_autopilot::mode
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
monitor_task
void monitor_task(void)
monitor stuff run at 1Hz
Definition: autopilot_firmware.c:194
slider_1_val
float slider_1_val
Definition: rc_settings.c:33
Square
#define Square(_x)
Definition: nav.h:53
stabilization_attitude.h
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
h_ctl_roll_setpoint
float h_ctl_roll_setpoint
Definition: stabilization_adaptive.c:157
send_energy
static void send_energy(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot_firmware.c:85
RadioControl::frame_rate
uint8_t frame_rate
Definition: radio_control.h:64
h_ctl_pitch_loop_setpoint
float h_ctl_pitch_loop_setpoint
Definition: stabilization_adaptive.c:171
telemetry.h
Electrical::charge
float charge
consumed electric charge in Ah
Definition: electrical.h:47
CATASTROPHIC_BAT_KILL_DELAY
#define CATASTROPHIC_BAT_KILL_DELAY
Maximum time allowed for catastrophic battery level before going into kill mode.
Definition: autopilot_firmware.c:180
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
desired_x
float desired_x
Definition: nav.c:309
lateral_mode
uint8_t lateral_mode
Definition: autopilot_firmware.c:38
desired_y
float desired_y
Definition: nav.c:309
pprz_autopilot::throttle
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
send_mode
static void send_mode(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot_firmware.c:56
fbw_mode
uint8_t fbw_mode
Fly by wire modes.
Definition: main_fbw.c:41
v_ctl_climb_setpoint
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:98
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
RadioControl::status
uint8_t status
Definition: radio_control.h:61
v_ctl_auto_airspeed_controlled
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:123
MIN_SPEED_FOR_TAKEOFF
#define MIN_SPEED_FOR_TAKEOFF
Default minimal speed for takeoff in m/s.
Definition: autopilot_firmware.c:190
KILL_MODE_DISTANCE
#define KILL_MODE_DISTANCE
Maximum distance from HOME waypoint before going into kill mode.
Definition: autopilot_firmware.c:185
autopilot_firmware_init
void autopilot_firmware_init(void)
Init function.
Definition: autopilot_firmware.c:152
autopilot_firmware.h
nav.h
v_ctl_auto_groundspeed_setpoint
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:125
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
v_ctl_altitude_setpoint
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
send_airspeed
static void send_airspeed(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot_firmware.c:107
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
RC_OK
#define RC_OK
Definition: radio_control.h:49
sys_time
Definition: sys_time.h:71
rc_settings_mode
uint8_t rc_settings_mode
Definition: autopilot_firmware.c:53
v_ctl_auto_airspeed_setpoint
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
AP_MODE_MANUAL
#define AP_MODE_MANUAL
AP modes.
Definition: autopilot_static.h:36
electrical
struct Electrical electrical
Definition: electrical.c:66
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
state.h
RcSettingsOff
#define RcSettingsOff()
Definition: rc_settings.h:70
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
Electrical::energy
float energy
consumed energy in Wh
Definition: electrical.h:48
send_estimator
static void send_estimator(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot_firmware.c:79
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
pprz_autopilot::kill_throttle
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
CONTROL_FREQUENCY
#define CONTROL_FREQUENCY
Definition: autopilot_static.h:60
horizontal_mode
uint8_t horizontal_mode
Definition: nav.c:73
RC_REALLY_LOST
#define RC_REALLY_LOST
Definition: radio_control.h:51
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
autopilot_send_mode
void autopilot_send_mode(void)
Report autopilot mode on default downlink channel.
Definition: autopilot_firmware.c:144
Electrical::current
float current
current in A
Definition: electrical.h:46
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
slider_2_val
float slider_2_val
Definition: rc_settings.c:33
h_ctl_course_setpoint
float h_ctl_course_setpoint
Definition: stabilization_adaptive.c:80