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_utils.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include "autopilot.h"
32 #include "state.h"
34 
36 PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
37 
38 
39 // Utility functions
40 #ifndef AUTOPILOT_DISABLE_AHRS_KILL
42 {
43  return stateIsAttitudeValid();
44 }
45 #else
46 PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
47 bool ap_ahrs_is_aligned(void)
48 {
49  return true;
50 }
51 #endif
52 
53 #if defined RADIO_MODE_2x3
54 
55 #define THRESHOLD_1d3_PPRZ (MAX_PPRZ / 3)
56 #define THRESHOLD_2d3_PPRZ ((MAX_PPRZ / 3) * 2)
57 
69 uint8_t ap_mode_of_3x2way_switch(void)
70 {
72  if (radio_control.values[RADIO_MODE] < 0) {
73  val = MAX_PPRZ + val;
74  }
75  if (val < THRESHOLD_1d3_PPRZ) {
76  return MODE_MANUAL;
77  } else if (val < THRESHOLD_2d3_PPRZ) {
78  return MODE_AUTO1;
79  } else {
80  return autopilot_mode_auto2;
81  }
82 }
83 
84 #else
85 
86 #define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
87 #define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
88 
91 {
93  return autopilot_mode_auto2;
95  return MODE_AUTO1;
96  } else {
97  return MODE_MANUAL;
98  }
99 }
100 
101 #endif
102 
111 #if defined RADIO_AUTO_MODE || defined(__DOXYGEN__)
112 uint8_t ap_mode_of_two_switches(void)
113 {
115  /* RADIO_MODE in MANUAL position */
116  return MODE_MANUAL;
117  } else {
118  /* RADIO_MODE not in MANUAL position.
119  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
120  */
121  if (radio_control.values[RADIO_AUTO_MODE] > THRESHOLD_2_PPRZ) {
122  return autopilot_mode_auto2;
123  } else {
124  return MODE_AUTO1;
125  }
126  }
127 }
128 #endif
129 
130 
135 void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight __attribute__((unused)), bool motors_on __attribute__((unused)))
136 {
137 #if !ROTORCRAFT_IS_HELI
138 #if !ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
139  if (!in_flight) {
140  cmd_in[COMMAND_YAW] = 0;
141  }
142 #endif
143  if (!motors_on) {
144  cmd_in[COMMAND_THRUST] = 0;
145  }
146 #endif
147  cmd_out[COMMAND_ROLL] = cmd_in[COMMAND_ROLL];
148  cmd_out[COMMAND_PITCH] = cmd_in[COMMAND_PITCH];
149  cmd_out[COMMAND_YAW] = cmd_in[COMMAND_YAW];
150  cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
151 }
152 
void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on)
Get autopilot mode from two 2way switches.
uint8_t ap_mode_of_3way_switch(void)
get autopilot mode as set by RADIO_MODE 3-way switch
#define FAILSAFE_DESCENT_SPEED
Set descent speed in failsafe mode.
Utility functions and includes for autopilots.
int16_t pprz_t
Definition: paparazzi.h:6
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:58
bool ap_ahrs_is_aligned(void)
Display descent speed in failsafe mode if needed.
#define MODE_MANUAL
Default RC mode.
Some helper functions to check RC sticks.
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
#define MODE_AUTO1
#define THRESHOLD_2_PPRZ
uint8_t autopilot_mode_auto2
uint16_t val[TCOUPLE_NB]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct RadioControl radio_control
Definition: radio_control.c:30
signed long int32_t
Definition: types.h:19
Core autopilot interface common to all firmwares.
#define RADIO_MODE
Definition: spektrum_arch.h:59
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define THRESHOLD_1_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8