Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
autopilot_static.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
3  * Copyright (C) 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 
29 #include "autopilot.h"
31 
32 #include "inter_mcu.h"
33 #include "link_mcu.h"
34 #include "state.h"
37 #include "generated/flight_plan.h"
38 
39 /* Geofence exceptions */
41 
42 #if USE_GPS
43 #include "subsystems/gps.h"
44 #endif
45 static bool gps_lost;
46 
48 
49 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
51 #endif
52 
53 static inline uint8_t pprz_mode_update(void);
54 static inline uint8_t mcu1_status_update(void);
55 static inline void copy_from_to_fbw(void);
56 
58 #ifndef RC_LOST_MODE
59 #define RC_LOST_MODE AP_MODE_HOME
60 #endif
61 
62 
64 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
65 #include "subsystems/abi.h"
66 volatile uint8_t new_ins_attitude = 0;
67 static abi_event new_att_ev;
68 static void new_att_cb(uint8_t sender_id __attribute__((unused)),
69  uint32_t stamp __attribute__((unused)),
70  struct Int32Rates *gyro __attribute__((unused)))
71 {
72  new_ins_attitude = 1;
73 }
74 
77 void autopilot_event(void)
78 {
79  if (new_ins_attitude > 0) {
80  attitude_loop();
81  new_ins_attitude = 0;
82  }
83 }
84 #endif
85 
86 
91 {
93 
95  gps_lost = false;
96 
98 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
99  AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &new_att_ev, &new_att_cb);
100 #endif
101 
102 }
103 
105 {
106 }
107 
112 {
113  uint8_t mode_changed = false;
115 
116  /* rc_lost_while_in_use is true if we lost RC in MANUAL or AUTO1 */
117  uint8_t rc_lost_while_in_use = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
119 
120  /* RC_LOST_MODE defaults to AP_MODE_HOME, but it can also be set to NAV_MODE_NAV or MANUAL when the RC receiver is well configured to send failsafe commands */
121  if (rc_lost_while_in_use) { // Always: no exceptions!
122  mode_changed = autopilot_set_mode(RC_LOST_MODE);
123  }
124 
125 #ifdef RADIO_KILL_SWITCH
126  if (imcu_get_radio(RADIO_KILL_SWITCH) < MIN_PPRZ / 2) {
128  }
129 #endif
130 
131  /* If in-flight, with good GPS but too far, then activate HOME mode
132  * In MANUAL with good RC, FBW will allow to override. */
135  mode_changed = autopilot_set_mode(AP_MODE_HOME);
136  }
137  }
138  if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) {
139  bool pprz_mode_changed = pprz_mode_update();
140  mode_changed |= pprz_mode_changed;
141 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
142  PPRZ_MUTEX_LOCK(fbw_state_mtx);
143  bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
144  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
145  rc_settings(calib_mode_changed || pprz_mode_changed);
146  mode_changed |= calib_mode_changed;
147 #endif
148  }
149  mode_changed |= mcu1_status_update();
150  if (mode_changed) { autopilot_send_mode(); }
151 
152 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
153 
158  h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
159 
161  h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH);
162 #if H_CTL_YAW_LOOP && defined RADIO_YAW
163 
164  h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
165 #endif
166  }
171  v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
172  }
175  mcu1_ppm_cpt = imcu_get_ppm_cpt();
176 #endif // RADIO_CONTROL
177 
178  // update electrical from FBW
179  imcu_get_electrical(&ap_electrical);
180 
181 #ifdef RADIO_CONTROL
182  /* the SITL check is a hack to prevent "automatic" launch in NPS */
183 #ifndef SITL
184  if (!autopilot.flight_time) {
186  autopilot.launch = true;
187  }
188  }
189 #endif
190 #endif
191 }
192 
193 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
194 {
195  if (new_autopilot_mode != autopilot.mode) {
196  autopilot.mode = new_autopilot_mode;
197  }
198 }
199 
200 void autopilot_static_SetModeHandler(float new_autopilot_mode)
201 {
202  autopilot_static_set_mode(new_autopilot_mode);
203 }
204 
205 void autopilot_static_set_motors_on(bool motors_on)
206 {
207  // it doesn't make real sense on fixedwing, as you can still use throttle
208  // in MAN and AUTO1 modes while have motor killed for AUTO2
209  // only needed for consistency with other firmwares
210  autopilot.motors_on = motors_on;
211 }
212 
213 #ifdef FAILSAFE_DELAY_WITHOUT_GPS
214 #define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
215 #endif
216 
220 void navigation_task(void)
221 {
222 #if defined FAILSAFE_DELAY_WITHOUT_GPS
223 
224  static uint8_t last_pprz_mode;
225 
228  if (autopilot.launch) {
229  if (GpsTimeoutError) {
231  last_pprz_mode = autopilot_get_mode();
234  gps_lost = true;
235  }
236  } else if (gps_lost) { /* GPS is ok */
238  autopilot_set_mode(last_pprz_mode);
239  gps_lost = false;
241  }
242  }
243 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
244 
246  if (autopilot_get_mode() == AP_MODE_HOME) {
247  nav_home();
249  nav_without_gps();
250  } else {
252  }
253 
254 #ifdef TCAS
255  callTCAS();
256 #endif
257 
258 #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
259  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
260 #endif
261 
262  /* The nav task computes only nav_altitude. However, we are interested
263  by desired_altitude (= nav_alt+alt_shift) in any case.
264  So we always run the altitude control loop */
267  }
268 
271 #ifdef H_CTL_RATE_LOOP
272  /* Be sure to be in attitude mode, not roll */
273  h_ctl_auto1_rate = false;
274 #endif
276  h_ctl_course_loop(); /* aka compute nav_desired_roll */
277  }
278 
279  // climb_loop(); //4Hz
280  }
281 }
282 
283 
284 void attitude_loop(void)
285 {
286 
288 #if CTRL_VERTICAL_LANDING
291  } else {
292 #endif
296  } else {
299  } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
300  } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
301 #if CTRL_VERTICAL_LANDING
302  } /* v_ctl_mode == V_CTL_MODE_LANDING */
303 #endif
304 
305 #if defined V_CTL_THROTTLE_IDLE
306  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
307 #endif
308 
309 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
310  if (ap_electrical.vsupply > 0.) {
311  v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply;
313  }
314 #endif
315 
316  // Copy the pitch setpoint from the guidance to the stabilization control
318  Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
321  }
322  }
323 
324  h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
326  PPRZ_MUTEX_LOCK(ap_state_mtx);
330  AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint);
331  AP_COMMAND_SET_CL(h_ctl_flaps_setpoint);
332  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
333 
334 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
335  link_mcu_send();
336 #elif defined INTER_MCU && defined SINGLE_MCU
337 
338  inter_mcu_received_ap = true;
339 #endif
340 
341 }
342 
343 /******************** Interaction with FBW *****************************/
344 
347 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
348 static inline uint8_t pprz_mode_update(void)
349 {
350  if ((autopilot_get_mode() != AP_MODE_HOME &&
352 #ifdef UNLOCKED_HOME_MODE
353  || true
354 #endif
355  ) {
356 #ifndef RADIO_AUTO_MODE
357  return autopilot_set_mode(AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
358 #else
359  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
360  /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
361  * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode selected by RADIO_AUTO_MODE.
362  *
363  * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
364  */
365  if (AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == AP_MODE_MANUAL) {
366  /* RADIO_MODE in MANUAL position */
368  } else {
369  /* RADIO_MODE not in MANUAL position.
370  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
371  */
372  return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1);
373  }
374 #endif // RADIO_AUTO_MODE
375  } else {
376  return false;
377  }
378 }
379 #else // not RADIO_CONTROL
380 static inline uint8_t pprz_mode_update(void)
381 {
382  return false;
383 }
384 #endif
385 
386 static inline uint8_t mcu1_status_update(void)
387 {
388  uint8_t new_status = imcu_get_status();
389  if (mcu1_status != new_status) {
390  bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED));
391  mcu1_status = new_status;
392  return changed;
393  }
394  return false;
395 }
396 
397 
400 static inline void copy_from_to_fbw(void)
401 {
402  PPRZ_MUTEX_LOCK(fbw_state_mtx);
403  PPRZ_MUTEX_LOCK(ap_state_mtx);
404 #ifdef SetAutoCommandsFromRC
405  SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
406 #elif defined RADIO_YAW && defined COMMAND_YAW
407  ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
408 #endif
409  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
410  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
411 }
static void copy_from_to_fbw(void)
Send back uncontrolled channels.
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void attitude_loop(void)
bool launch
request launch
Definition: autopilot.h:71
uint8_t mcu1_status
Second MCU status (FBW part)
#define AP_COMMAND_SET_PITCH(_pitch)
angular rates
void h_ctl_course_loop(void)
#define RADIO_ROLL
Definition: intermcu_ap.h:41
#define AP_MODE_AUTO1
Communication between fbw and ap processes.
#define AP_COMMAND_SET_CL(_cl)
uint8_t lateral_mode
static uint8_t mcu1_status_update(void)
void h_ctl_attitude_loop(void)
#define MIN_PPRZ
Definition: paparazzi.h:9
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
#define RC_LOST_MODE
mode to enter when RC is lost in AP_MODE_MANUAL or AP_MODE_AUTO1
#define THROTTLE_THRESHOLD_TAKEOFF
Takeoff detection threshold from throttle.
void callTCAS(void)
Definition: tcas.c:65
static bool gps_lost
Main include for ABI (AirBorneInterface).
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
void autopilot_static_SetModeHandler(float new_autopilot_mode)
void navigation_task(void)
Compute desired_course.
float vsupply
supply voltage in V
Definition: electrical.h:45
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
#define V_CTL_MODE_AUTO_THROTTLE
#define AP_MODE_HOME
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:161
pprz_t h_ctl_elevator_setpoint
pprz_t h_ctl_aileron_setpoint
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:452
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:336
#define AP_COMMAND_SET_ROLL(_roll)
AP command setter macros for usual commands.
struct fbw_state * fbw_state
Definition: inter_mcu.c:36
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:133
struct ap_state * ap_state
Definition: inter_mcu.c:37
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
bool too_far_from_home
Definition: navigation.c:87
Fixed wing horizontal control.
#define LATERAL_MODE_COURSE
#define AP_MODE_MANUAL
AP modes.
bool h_ctl_auto1_rate
#define UNLOCKED_HOME_MODE
static uint8_t mcu1_ppm_cpt
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition: autopilot.c:306
void common_nav_periodic_task_4Hz()
Definition: common_nav.c:152
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#define RADIO_MODE
Definition: intermcu_ap.h:44
#define AP_MODE_AUTO2
#define V_CTL_MODE_AUTO_ALT
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
void autopilot_static_periodic(void)
void rc_settings(bool mode_changed)
Includes generated code from tuning_rc.xml.
Definition: rc_settings.c:48
Device independent GPS code (interface)
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:219
static uint8_t pprz_mode_update(void)
Update paparazzi mode.
#define FLOAT_OF_PPRZ(pprz, center, travel)
pprz_t to float with saturation
#define LATERAL_MODE_MANUAL
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:146
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
#define V_CTL_MODE_AUTO_CLIMB
void autopilot_static_init(void)
Static autopilot API.
void v_ctl_landing_loop(void)
Definition: guidance_v.c:357
Core autopilot interface common to all firmwares.
#define THRESHOLD2
float h_ctl_pitch_setpoint
bool motors_on
motor status
Definition: autopilot.h:68
void autopilot_static_set_motors_on(bool motors_on)
API to get/set the generic vehicle states.
volatile uint8_t new_ins_attitude
#define RADIO_PITCH
Definition: intermcu_ap.h:42
float h_ctl_roll_setpoint
#define AP_MODE_GPS_OUT_OF_ORDER
#define RADIO_YAW
Definition: intermcu_ap.h:43
#define AP_MODE_OF_PULSE(pprz)
Get mode from pulse.
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
#define MAX_PPRZ
Definition: paparazzi.h:8
#define AP_COMMAND_SET_THROTTLE(_throttle)
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
struct Electrical ap_electrical
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
Fixedwing autopilot modes (static implementation).
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
#define AP_COMMAND_SET_YAW(_yaw)
#define V_CTL_MODE_LANDING
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:298
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78