Paparazzi UAS  v5.12_stable-4-g9b43e9b
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  /* really_lost is true if we lost RC in MANUAL or AUTO1 */
118  uint8_t really_lost = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
120 
123  mode_changed = autopilot_set_mode(AP_MODE_HOME);
124  }
125  if (really_lost) {
126  mode_changed = autopilot_set_mode(RC_LOST_MODE);
127  }
128  }
129  if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) {
130  bool pprz_mode_changed = pprz_mode_update();
131  mode_changed |= pprz_mode_changed;
132 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
133  PPRZ_MUTEX_LOCK(fbw_state_mtx);
134  bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
135  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
136  rc_settings(calib_mode_changed || pprz_mode_changed);
137  mode_changed |= calib_mode_changed;
138 #endif
139  }
140  mode_changed |= mcu1_status_update();
141  if (mode_changed) { autopilot_send_mode(); }
142 
143 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
144 
149  h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
150 
152  h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH);
153 #if H_CTL_YAW_LOOP && defined RADIO_YAW
154 
155  h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
156 #endif
157  }
162  v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
163  }
166  mcu1_ppm_cpt = imcu_get_ppm_cpt();
167 #endif // RADIO_CONTROL
168 
169  // update electrical from FBW
170  imcu_get_electrical(&vsupply, &current, &energy);
171 
172 #ifdef RADIO_CONTROL
173  /* the SITL check is a hack to prevent "automatic" launch in NPS */
174 #ifndef SITL
175  if (!autopilot.flight_time) {
177  autopilot.launch = true;
178  }
179  }
180 #endif
181 #endif
182 }
183 
184 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
185 {
186  if (new_autopilot_mode != autopilot.mode) {
187  autopilot.mode = new_autopilot_mode;
188  }
189 }
190 
191 void autopilot_static_SetModeHandler(float new_autopilot_mode)
192 {
193  autopilot_static_set_mode(new_autopilot_mode);
194 }
195 
196 void autopilot_static_set_motors_on(bool motors_on __attribute__((unused)))
197 {
198  // Do nothing on fixedwing ?
199 }
200 
201 #ifdef FAILSAFE_DELAY_WITHOUT_GPS
202 #define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
203 #endif
204 
208 void navigation_task(void)
209 {
210 #if defined FAILSAFE_DELAY_WITHOUT_GPS
211 
212  static uint8_t last_pprz_mode;
213 
216  if (autopilot.launch) {
217  if (GpsTimeoutError) {
219  last_pprz_mode = autopilot_get_mode();
222  gps_lost = true;
223  }
224  } else if (gps_lost) { /* GPS is ok */
226  autopilot_set_mode(last_pprz_mode);
227  gps_lost = false;
229  }
230  }
231 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
232 
234  if (autopilot_get_mode() == AP_MODE_HOME) {
235  nav_home();
237  nav_without_gps();
238  } else {
240  }
241 
242 #ifdef TCAS
243  callTCAS();
244 #endif
245 
246 #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
247  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
248 #endif
249 
250  /* The nav task computes only nav_altitude. However, we are interested
251  by desired_altitude (= nav_alt+alt_shift) in any case.
252  So we always run the altitude control loop */
255  }
256 
259 #ifdef H_CTL_RATE_LOOP
260  /* Be sure to be in attitude mode, not roll */
261  h_ctl_auto1_rate = false;
262 #endif
264  h_ctl_course_loop(); /* aka compute nav_desired_roll */
265  }
266 
267  // climb_loop(); //4Hz
268  }
269 }
270 
271 
272 void attitude_loop(void)
273 {
274 
276 #if CTRL_VERTICAL_LANDING
279  } else {
280 #endif
284  } else {
287  } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
288  } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
289 #if CTRL_VERTICAL_LANDING
290  } /* v_ctl_mode == V_CTL_MODE_LANDING */
291 #endif
292 
293 #if defined V_CTL_THROTTLE_IDLE
294  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
295 #endif
296 
297 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
298  if (vsupply > 0.) {
299  v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
301  }
302 #endif
303 
304  // Copy the pitch setpoint from the guidance to the stabilization control
306  Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
309  }
310  }
311 
312  h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
314  PPRZ_MUTEX_LOCK(ap_state_mtx);
315  ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
316  ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
317  ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
318 #if H_CTL_YAW_LOOP && defined COMMAND_YAW
319  ap_state->commands[COMMAND_YAW] = h_ctl_rudder_setpoint;
320 #endif
321 #if H_CTL_CL_LOOP && defined COMMAND_CL
322  ap_state->commands[COMMAND_CL] = h_ctl_flaps_setpoint;
323 #endif
324  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
325 
326 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
327  link_mcu_send();
328 #elif defined INTER_MCU && defined SINGLE_MCU
329 
330  inter_mcu_received_ap = true;
331 #endif
332 
333 }
334 
335 /******************** Interaction with FBW *****************************/
336 
339 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
340 static inline uint8_t pprz_mode_update(void)
341 {
342  if ((autopilot_get_mode() != AP_MODE_HOME &&
344 #ifdef UNLOCKED_HOME_MODE
345  || true
346 #endif
347  ) {
348 #ifndef RADIO_AUTO_MODE
349  return autopilot_set_mode(AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
350 #else
351  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
352  /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
353  * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode selected by RADIO_AUTO_MODE.
354  *
355  * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
356  */
357  if (AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == AP_MODE_MANUAL) {
358  /* RADIO_MODE in MANUAL position */
360  } else {
361  /* RADIO_MODE not in MANUAL position.
362  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
363  */
364  return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1);
365  }
366 #endif // RADIO_AUTO_MODE
367  } else {
368  return false;
369  }
370 }
371 #else // not RADIO_CONTROL
372 static inline uint8_t pprz_mode_update(void)
373 {
374  return false;
375 }
376 #endif
377 
378 static inline uint8_t mcu1_status_update(void)
379 {
380  uint8_t new_status = imcu_get_status();
381  if (mcu1_status != new_status) {
382  bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED));
383  mcu1_status = new_status;
384  return changed;
385  }
386  return false;
387 }
388 
389 
392 static inline void copy_from_to_fbw(void)
393 {
394  PPRZ_MUTEX_LOCK(fbw_state_mtx);
395  PPRZ_MUTEX_LOCK(ap_state_mtx);
396 #ifdef SetAutoCommandsFromRC
397  SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
398 #elif defined RADIO_YAW && defined COMMAND_YAW
399  ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
400 #endif
401  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
402  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
403 }
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:65
uint8_t mcu1_status
Second MCU status (FBW part)
angular rates
void h_ctl_course_loop(void)
#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)
#define RADIO_ROLL
Definition: spektrum_arch.h:43
#define RADIO_YAW
Definition: spektrum_arch.h:45
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:156
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:450
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:334
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)
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:301
void common_nav_periodic_task_4Hz()
Definition: common_nav.c:117
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#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
#define RADIO_PITCH
Definition: spektrum_arch.h:44
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 RADIO_THROTTLE
Definition: spektrum_arch.h:42
#define LATERAL_MODE_MANUAL
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:141
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#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 RADIO_MODE
Definition: spektrum_arch.h:59
#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
float h_ctl_roll_setpoint
#define AP_MODE_GPS_OUT_OF_ORDER
#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:63
#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:179
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:296