Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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  attitude_loop();
107 }
108 
113 {
114  uint8_t mode_changed = false;
116 
117  /* rc_lost_while_in_use is true if we lost RC in MANUAL or AUTO1 */
118  uint8_t rc_lost_while_in_use = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
120 
121  /* 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 */
122  if (rc_lost_while_in_use) { // Always: no exceptions!
123  mode_changed = autopilot_set_mode(RC_LOST_MODE);
124  }
125 
126  /* If in-flight, with good GPS but too far, then activate HOME mode
127  * In MANUAL with good RC, FBW will allow to override. */
130  mode_changed = autopilot_set_mode(AP_MODE_HOME);
131  }
132  }
133  if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) {
134  bool pprz_mode_changed = pprz_mode_update();
135  mode_changed |= pprz_mode_changed;
136 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
137  PPRZ_MUTEX_LOCK(fbw_state_mtx);
138  bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
139  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
140  rc_settings(calib_mode_changed || pprz_mode_changed);
141  mode_changed |= calib_mode_changed;
142 #endif
143  }
144  mode_changed |= mcu1_status_update();
145  if (mode_changed) { autopilot_send_mode(); }
146 
147 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
148 
153  h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
154 
156  h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH);
157 #if H_CTL_YAW_LOOP && defined RADIO_YAW
158 
159  h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
160 #endif
161  }
166  v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
167  }
170  mcu1_ppm_cpt = imcu_get_ppm_cpt();
171 #endif // RADIO_CONTROL
172 
173  // update electrical from FBW
174  imcu_get_electrical(&vsupply, &current, &energy);
175 
176 #ifdef RADIO_CONTROL
177  /* the SITL check is a hack to prevent "automatic" launch in NPS */
178 #ifndef SITL
179  if (!autopilot.flight_time) {
181  autopilot.launch = true;
182  }
183  }
184 #endif
185 #endif
186 }
187 
188 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
189 {
190  if (new_autopilot_mode != autopilot.mode) {
191  autopilot.mode = new_autopilot_mode;
192  }
193 }
194 
195 void autopilot_static_SetModeHandler(float new_autopilot_mode)
196 {
197  autopilot_static_set_mode(new_autopilot_mode);
198 }
199 
200 void autopilot_static_set_motors_on(bool motors_on __attribute__((unused)))
201 {
202  // Do nothing on fixedwing ?
203 }
204 
205 #ifdef FAILSAFE_DELAY_WITHOUT_GPS
206 #define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
207 #endif
208 
212 void navigation_task(void)
213 {
214 #if defined FAILSAFE_DELAY_WITHOUT_GPS
215 
216  static uint8_t last_pprz_mode;
217 
220  if (autopilot.launch) {
221  if (GpsTimeoutError) {
223  last_pprz_mode = autopilot_get_mode();
226  gps_lost = true;
227  }
228  } else if (gps_lost) { /* GPS is ok */
230  autopilot_set_mode(last_pprz_mode);
231  gps_lost = false;
233  }
234  }
235 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
236 
238  if (autopilot_get_mode() == AP_MODE_HOME) {
239  nav_home();
241  nav_without_gps();
242  } else {
244  }
245 
246 #ifdef TCAS
247  callTCAS();
248 #endif
249 
250 #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
251  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
252 #endif
253 
254  /* The nav task computes only nav_altitude. However, we are interested
255  by desired_altitude (= nav_alt+alt_shift) in any case.
256  So we always run the altitude control loop */
259  }
260 
263 #ifdef H_CTL_RATE_LOOP
264  /* Be sure to be in attitude mode, not roll */
265  h_ctl_auto1_rate = false;
266 #endif
268  h_ctl_course_loop(); /* aka compute nav_desired_roll */
269  }
270 
271  // climb_loop(); //4Hz
272  }
273 }
274 
275 
276 void attitude_loop(void)
277 {
278 
280 #if CTRL_VERTICAL_LANDING
283  } else {
284 #endif
288  } else {
291  } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
292  } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
293 #if CTRL_VERTICAL_LANDING
294  } /* v_ctl_mode == V_CTL_MODE_LANDING */
295 #endif
296 
297 #if defined V_CTL_THROTTLE_IDLE
298  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
299 #endif
300 
301 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
302  if (vsupply > 0.) {
303  v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
305  }
306 #endif
307 
308  // Copy the pitch setpoint from the guidance to the stabilization control
310  Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
313  }
314  }
315 
316  h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
318  PPRZ_MUTEX_LOCK(ap_state_mtx);
319  ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
320  ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
321  ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
322 #if H_CTL_YAW_LOOP && defined COMMAND_YAW
323  ap_state->commands[COMMAND_YAW] = h_ctl_rudder_setpoint;
324 #endif
325 #if H_CTL_CL_LOOP && defined COMMAND_CL
326  ap_state->commands[COMMAND_CL] = h_ctl_flaps_setpoint;
327 #endif
328  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
329 
330 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
331  link_mcu_send();
332 #elif defined INTER_MCU && defined SINGLE_MCU
333 
334  inter_mcu_received_ap = true;
335 #endif
336 
337 }
338 
339 /******************** Interaction with FBW *****************************/
340 
343 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
344 static inline uint8_t pprz_mode_update(void)
345 {
346  if ((autopilot_get_mode() != AP_MODE_HOME &&
348 #ifdef UNLOCKED_HOME_MODE
349  || true
350 #endif
351  ) {
352 #ifndef RADIO_AUTO_MODE
353  return autopilot_set_mode(AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
354 #else
355  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
356  /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
357  * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode selected by RADIO_AUTO_MODE.
358  *
359  * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
360  */
361  if (AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == AP_MODE_MANUAL) {
362  /* RADIO_MODE in MANUAL position */
364  } else {
365  /* RADIO_MODE not in MANUAL position.
366  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
367  */
368  return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1);
369  }
370 #endif // RADIO_AUTO_MODE
371  } else {
372  return false;
373  }
374 }
375 #else // not RADIO_CONTROL
376 static inline uint8_t pprz_mode_update(void)
377 {
378  return false;
379 }
380 #endif
381 
382 static inline uint8_t mcu1_status_update(void)
383 {
384  uint8_t new_status = imcu_get_status();
385  if (mcu1_status != new_status) {
386  bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED));
387  mcu1_status = new_status;
388  return changed;
389  }
390  return false;
391 }
392 
393 
396 static inline void copy_from_to_fbw(void)
397 {
398  PPRZ_MUTEX_LOCK(fbw_state_mtx);
399  PPRZ_MUTEX_LOCK(ap_state_mtx);
400 #ifdef SetAutoCommandsFromRC
401  SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
402 #elif defined RADIO_YAW && defined COMMAND_YAW
403  ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
404 #endif
405  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
406  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
407 }
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:66
uint8_t mcu1_status
Second MCU status (FBW part)
angular rates
void h_ctl_course_loop(void)
#define RADIO_ROLL
Definition: intermcu_ap.h:40
#define AP_MODE_AUTO1
Communication between fbw and ap processes.
uint8_t lateral_mode
static uint8_t mcu1_status_update(void)
void h_ctl_attitude_loop(void)
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
int32_t current
Supply current in milliAmpere.
#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:61
void autopilot_static_SetModeHandler(float new_autopilot_mode)
void navigation_task(void)
Compute desired_course.
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:160
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
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:83
Fixed wing horizontal control.
float energy
Energy consumption (mAh) This is the ap copy of the measurement from fbw FIXME use electrical module ...
#define LATERAL_MODE_COURSE
uint16_t vsupply
Supply voltage in deciVolt.
#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:305
void common_nav_periodic_task_4Hz()
Definition: common_nav.c:117
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#define RADIO_MODE
Definition: intermcu_ap.h:43
#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:49
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
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:145
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define RADIO_THROTTLE
Definition: intermcu_ap.h:39
#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
unsigned char uint8_t
Definition: types.h:14
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:41
float h_ctl_roll_setpoint
#define AP_MODE_GPS_OUT_OF_ORDER
#define RADIO_YAW
Definition: intermcu_ap.h:42
#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
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:64
#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:59
#define V_CTL_MODE_LANDING
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:183
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:298