Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
autopilot_static.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2012 The Paparazzi Team
3 * Copyright (C) 2016 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, write to
19 * the Free Software Foundation, 59 Temple Place - Suite 330,
20 * Boston, MA 02111-1307, USA.
21 */
22
30#include "autopilot.h"
31#include "autopilot_arming.h"
32
40
43
44#if USE_STABILIZATION_RATE
46#endif
47
49
50#include "generated/settings.h"
51
52#if USE_GPS
53#include "modules/gps/gps.h"
54#else
55#if NO_GPS_NEEDED_FOR_NAV
56#define GpsIsLost() FALSE
57#else
58#define GpsIsLost() TRUE
59#endif
60#endif
61
62/* Geofence exceptions */
64
66#ifndef FAILSAFE_MODE_TOO_FAR_FROM_HOME
67#define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
68#endif
69
70
71#ifndef MODE_STARTUP
72#define MODE_STARTUP AP_MODE_KILL
73PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
74#endif
75
76#ifndef UNLOCKED_HOME_MODE
77#if MODE_AUTO1 == AP_MODE_HOME
78#define UNLOCKED_HOME_MODE TRUE
79PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO1 is AP_MODE_HOME")
80#elif MODE_AUTO2 == AP_MODE_HOME
81#define UNLOCKED_HOME_MODE TRUE
82PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
83#else
84#define UNLOCKED_HOME_MODE FALSE
85#endif
86#endif
87
88#if MODE_MANUAL == AP_MODE_NAV
89#error "MODE_MANUAL mustn't be AP_MODE_NAV"
90#endif
91
92
94{
95 /* Mode is finally set by autopilot_static_set_mode if MODE_STARTUP is not KILL.
96 * For autopilot_static_set_mode to do anything, the requested mode needs to differ
97 * from previous mode, so we set it to a safe KILL first.
98 */
100
101 /* set startup mode, propagates through to guidance h/v */
103
104 /* init arming */
106}
107
108
109#define NAV_PRESCALER (PERIODIC_FREQUENCY / NAVIGATION_FREQUENCY)
111{
112
114
119 } else {
121 }
122 }
123 }
124
125 if (autopilot.mode == AP_MODE_HOME) {
127 } else {
128 // otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
130 }
131
132
133 /* If in FAILSAFE mode and either already not in_flight anymore
134 * or just "detected" ground, go to KILL mode.
135 */
137 if (!autopilot_in_flight()) {
139 }
140
141#if FAILSAFE_GROUND_DETECT
142 INFO("Using FAILSAFE_GROUND_DETECT: KILL")
145 }
146#endif
147 }
148
149 /* Reset ground detection _after_ running flight plan
150 */
151 if (!autopilot_in_flight()) {
154 }
155
156 /* Set fixed "failsafe" commands from airframe file if in KILL mode.
157 * If in FAILSAFE mode, run normal loops with failsafe attitude and
158 * downwards velocity setpoints.
159 */
162 switch (autopilot.mode) {
163 case AP_MODE_FAILSAFE:
164#ifndef KILL_AS_FAILSAFE
169 break;
170#endif
171 case AP_MODE_KILL:
173 break;
174 default:
178 } else {
180 }
182 // TODO maybe add RC limiter here as an option ?
184 break;
185 }
186#ifdef COMMAND_THRUST
188#endif
189
190}
191
197{
199 // safety modes are always accessible via settings
201 } else {
202 if (radio_control.status != RC_OK &&
203 (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED)) {
204 // without RC, only nav-like modes are accessible
206 }
207 }
208 // with RC, other modes can only be changed from the RC
209}
210
211
213{
215 /* horizontal mode */
216 switch (new_autopilot_mode) {
217 case AP_MODE_FAILSAFE:
218#ifndef KILL_AS_FAILSAFE
222 break;
223#endif
224 case AP_MODE_KILL:
228 break;
232 break;
236#if USE_STABILIZATION_RATE
239#else
240 return;
241#endif
242 break;
250 break;
251 case AP_MODE_FORWARD:
255 break;
260 break;
267 break;
268 case AP_MODE_HOME:
269 case AP_MODE_NAV:
273 break;
274 case AP_MODE_GUIDED:
278 break;
279 default:
280 break;
281 }
282 /* vertical mode */
283 switch (new_autopilot_mode) {
284 case AP_MODE_FAILSAFE:
285#ifndef KILL_AS_FAILSAFE
288 break;
289#endif
290 case AP_MODE_KILL:
292#ifdef COMMAND_THRUST
293 stabilization.cmd[COMMAND_THRUST] = 0; // FIXME maybe not needed ?
294#endif
296 break;
302 case AP_MODE_FORWARD:
304 break;
308 break;
312 break;
317 break;
318 case AP_MODE_HOME:
319 case AP_MODE_NAV:
321 break;
322 case AP_MODE_GUIDED:
324 break;
325 default:
326 break;
327 }
328 //if switching to rate mode but rate mode is not defined, the function returned
330 }
331}
332
333
335{
336 if (autopilot.mode != AP_MODE_KILL && motors_on) {
337 autopilot.motors_on = true;
338 } else {
339 autopilot.motors_on = false;
340 }
342}
343
345{
346
347 if (kill_switch_is_on()) {
349 } else {
350#ifdef RADIO_AUTO_MODE
351 INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
353#else
354#ifdef RADIO_MODE_2x3
356#else
358#endif
359#endif
360
361 /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
363 /* always allow to switch to manual */
366 }
367 /* if in HOME or FAILSAFE mode, don't allow switching to non-manual modes */
369
371 /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
373#endif
374 ) {
376 }
377 }
378 }
379
380 /* an arming sequence is used to start/stop motors.
381 */
384
385 /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
387
388 /* if there are some commands that should always be set from RC, do it */
389#ifdef SetAutoCommandsFromRC
391#endif
392
393 /* if not in NAV_MODE set commands from the rc */
394#ifdef SetCommandsFromRC
395 if (autopilot.mode != AP_MODE_NAV) {
397 }
398#endif
399 }
400
401}
402
404#ifndef RC_LOST_MODE
405#define RC_LOST_MODE AP_MODE_FAILSAFE
406#endif
407
446
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition autopilot.c:193
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition autopilot.c:222
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition autopilot.c:330
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Definition autopilot.c:250
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:49
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:295
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:340
Core autopilot interface common to all firmwares.
bool motors_on
motor status
Definition autopilot.h:68
bool ground_detected
automatic detection of landing
Definition autopilot.h:73
pprz_t throttle
throttle level as will be displayed in GCS
Definition autopilot.h:66
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition autopilot.h:74
bool kill_throttle
allow autopilot to use throttle
Definition autopilot.h:69
uint8_t mode
current autopilot mode
Definition autopilot.h:63
bool in_flight
in flight status
Definition autopilot.h:70
Arming procedure for rotorcraft Several options can be selected:
static void autopilot_arming_set(bool motors_on)
static void autopilot_arming_check_motors_on(void)
State machine to check if motors should be turned ON or OFF using the kill switch.
static void autopilot_arming_init(void)
Autopilot guided mode interface.
const pprz_t commands_failsafe[COMMANDS_NB]
Definition commands.c:31
Hardware independent code for commands handling.
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
bool bat_critical
battery critical status
Definition electrical.h:51
void autopilot_static_init(void)
Static autopilot API.
void autopilot_static_periodic(void)
void autopilot_static_SetModeHandler(float new_autopilot_mode)
void autopilot_static_set_motors_on(bool motors_on)
#define RC_LOST_MODE
mode to enter when RC is lost in AP_MODE_MANUAL or AP_MODE_AUTO1
void autopilot_failsafe_checks(void)
Failsafe checks.
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
#define AP_MODE_HOME
Device independent GPS code (interface)
#define GpsIsLost()
Definition gps.h:170
struct ThrustSetpoint thrust_sp
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
uint16_t foo
Definition main_demo5.c:58
Hardware independent API for actuators (servos, motor controllers).
void nav_home(void)
Home mode navigation (circle around HOME)
Definition nav.c:424
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition nav.c:445
Optional exceptions triggeringg HOME_MODE 1) GEOFENCE_DATALINK_LOST_TIME: go to HOME mode if datalink...
static bool higher_than_max_altitude(void)
static bool datalink_lost(void)
struct RadioControl radio_control
Generic interface for radio control modules.
#define RC_REALLY_LOST
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#define RC_OK
static bool kill_switch_is_on(void)
#define FAILSAFE_MODE_TOO_FAR_FROM_HOME
Mode that is set when the plane is really too far from home.
#define NAV_PRESCALER
#define MODE_STARTUP
#define UNLOCKED_HOME_MODE
#define AP_MODE_HOVER_DIRECT
#define AP_MODE_RATE_DIRECT
#define AP_MODE_RATE_RC_CLIMB
#define AP_MODE_HOVER_CLIMB
#define AP_MODE_HOVER_Z_HOLD
#define AP_MODE_RC_DIRECT
#define AP_MODE_ATTITUDE_DIRECT
#define AP_MODE_FORWARD
#define AP_MODE_ATTITUDE_CLIMB
#define AP_MODE_NAV
#define AP_MODE_GUIDED
#define AP_MODE_ATTITUDE_Z_HOLD
#define MODE_MANUAL
Default RC mode.
#define AP_MODE_KILL
Static autopilot modes.
#define AP_MODE_FAILSAFE
#define AP_MODE_ATTITUDE_RC_CLIMB
#define AP_MODE_RATE_Z_HOLD
#define AP_MODE_CARE_FREE_DIRECT
uint8_t ap_mode_of_3way_switch(void)
get autopilot mode as set by RADIO_MODE 3-way switch
#define SetRotorcraftCommands(_cmd, _in_flight, _motors_on)
#define FAILSAFE_DESCENT_SPEED
Set descent speed in failsafe mode.
void guidance_h_mode_changed(uint8_t new_mode)
Definition guidance_h.c:128
struct HorizontalGuidance guidance_h
Definition guidance_h.c:45
struct StabilizationSetpoint guidance_h_run(bool in_flight)
Definition guidance_h.c:244
#define GUIDANCE_H_MODE_NAV
Definition guidance_h.h:58
#define GUIDANCE_H_MODE_HOVER
Definition guidance_h.h:57
#define GUIDANCE_H_MODE_NONE
Definition guidance_h.h:56
#define GUIDANCE_H_MODE_GUIDED
Definition guidance_h.h:59
void guidance_v_set_vz(float vz)
Set z velocity setpoint.
Definition guidance_v.c:378
void guidance_v_mode_changed(uint8_t new_mode)
Definition guidance_v.c:132
struct ThrustSetpoint guidance_v_run(bool in_flight)
Guidance vertical run functions.
Definition guidance_v.c:221
#define GUIDANCE_V_MODE_CLIMB
Definition guidance_v.h:38
#define GUIDANCE_V_MODE_RC_CLIMB
Definition guidance_v.h:37
#define GUIDANCE_V_MODE_KILL
Definition guidance_v.h:35
#define GUIDANCE_V_MODE_RC_DIRECT
Definition guidance_v.h:36
#define GUIDANCE_V_MODE_NAV
Definition guidance_v.h:40
#define GUIDANCE_V_MODE_HOVER
Definition guidance_v.h:39
#define GUIDANCE_V_MODE_GUIDED
Definition guidance_v.h:41
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition navigation.c:326
struct RotorcraftNavigation nav
Definition navigation.c:51
Rotorcraft navigation functions.
float dist2_to_home
squared distance to home waypoint
Definition navigation.h:143
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition navigation.h:145
bool too_far_from_home
too_far flag
Definition navigation.h:144
General attitude stabilization interface for rotorcrafts.
Persistent settings interface.
static const ShellCommand commands[]
Definition shell_arch.c:78
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition sonar_bebop.c:65
struct Stabilization stabilization
void stabilization_mode_changed(uint8_t new_mode, uint8_t submode)
Check mode change.
void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Call default stabilization control.
struct StabilizationSetpoint stabilization_get_failsafe_sp(void)
Get stabilization setpoint for failsafe.
General stabilization interface for rotorcrafts.
#define STABILIZATION_ATT_SUBMODE_CARE_FREE
#define STABILIZATION_MODE_ATTITUDE
#define STABILIZATION_ATT_SUBMODE_HEADING
Stabilization sub-modes for attitude.
#define STABILIZATION_ATT_SUBMODE_FORWARD
struct StabilizationSetpoint rc_sp
Keep it ? FIXME.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
#define STABILIZATION_MODE_RATE
#define STABILIZATION_MODE_DIRECT
#define STABILIZATION_MODE_NONE
Stabilization modes.
Rate stabilization for rotorcrafts.
Stabilization setpoint.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.