Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_rate_indi.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
3  * 2020 Rohan Chotalal
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 
29 
30 #ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
32 #else
34 #endif
35 
37 
38 /* -- define variables */
40 
42 #ifndef STABILIZATION_INDI_MAX_RATE
43 #define STABILIZATION_INDI_MAX_RATE 6.0
44 #endif
45 
46 /* -- RC deadbands */
47 #ifndef STABILIZATION_RATE_DEADBAND_P
48 #define STABILIZATION_RATE_DEADBAND_P 0
49 #endif
50 #ifndef STABILIZATION_RATE_DEADBAND_Q
51 #define STABILIZATION_RATE_DEADBAND_Q 0
52 #endif
53 #ifndef STABILIZATION_RATE_DEADBAND_R
54 #define STABILIZATION_RATE_DEADBAND_R 200
55 #endif
56 
57 #define ROLL_RATE_DEADBAND_EXCEEDED() \
58  (radio_control.values[RADIO_ROLL] > STABILIZATION_RATE_DEADBAND_P || \
59  radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
60 
61 #define PITCH_RATE_DEADBAND_EXCEEDED() \
62  (radio_control.values[RADIO_PITCH] > STABILIZATION_RATE_DEADBAND_Q || \
63  radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
64 
65 #define YAW_RATE_DEADBAND_EXCEEDED() \
66  (radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \
67  radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
68 
69 
70 #if PERIODIC_TELEMETRY
72 
73 static void send_rate(struct transport_tx *trans, struct link_device *dev)
74 {
75  float dummy = 0;
76  float fb_p = stabilization_cmd[COMMAND_ROLL];
77  float fb_q = stabilization_cmd[COMMAND_PITCH];
78  float fb_r = stabilization_cmd[COMMAND_YAW];
79 
80  pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
84  &dummy, &dummy, &dummy,
85  &fb_p,
86  &fb_q,
87  &fb_r,
88  &stabilization_cmd[COMMAND_THRUST]);
89 }
90 #endif
91 
96 {
98 
99 #if PERIODIC_TELEMETRY
100  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RATE_LOOP, send_rate);
101 #endif
102 }
103 
108 {
110 }
111 
116 {
119  } else {
121  }
122 
125  } else {
127  }
128 
131  } else {
133  }
134 }
135 
141 {
144  } else {
146  }
147 
150  } else {
152  }
153 
156  } else {
158  }
159 }
160 
164 void stabilization_rate_run(bool in_flight)
165 {
166  /* compute the INDI rate command */
168 }
ROLL_RATE_DEADBAND_EXCEEDED
#define ROLL_RATE_DEADBAND_EXCEEDED()
Definition: stabilization_rate_indi.c:57
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
PITCH_RATE_DEADBAND_EXCEEDED
#define PITCH_RATE_DEADBAND_EXCEEDED()
Definition: stabilization_rate_indi.c:61
stabilization_indi_init
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
Definition: stabilization_indi.c:225
YAW_RATE_DEADBAND_EXCEEDED
#define YAW_RATE_DEADBAND_EXCEEDED()
Definition: stabilization_rate_indi.c:65
THROTTLE_STICK_DOWN
#define THROTTLE_STICK_DOWN()
Definition: autopilot_rc_helpers.h:41
stabilization_rate_read_rc
void stabilization_rate_read_rc(void)
Read RC comands with roll and yaw sticks.
Definition: stabilization_rate_indi.c:115
stabilization_indi_rate_run
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
Definition: stabilization_indi.c:373
stabilization_indi.h
telemetry.h
stabilization_indi_enter
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
Definition: stabilization_indi.c:275
stabilization_rate_read_rc_switched_sticks
void stabilization_rate_read_rc_switched_sticks(void)
Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks ...
Definition: stabilization_rate_indi.c:140
autopilot_rc_helpers.h
stabilization_rate_run
void stabilization_rate_run(bool in_flight)
Run indi rate interface from the "stabilization_rate_run" function.
Definition: stabilization_rate_indi.c:164
stabilization_indi_simple.h
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stabilization_rate_init
void stabilization_rate_init(void)
Initialize rate controller.
Definition: stabilization_rate_indi.c:95
stabilization_rate_sp
struct FloatRates stabilization_rate_sp
Definition: stabilization_rate_indi.c:39
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
STABILIZATION_INDI_MAX_RATE
#define STABILIZATION_INDI_MAX_RATE
Maximum rate you can request in RC rate mode (rad/s)
Definition: stabilization_rate_indi.c:43
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
send_rate
static void send_rate(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_rate_indi.c:73
stabilization_rate_indi.h
stabilization.h
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
stabilization_rate_enter
void stabilization_rate_enter(void)
Reset rate controller.
Definition: stabilization_rate_indi.c:107
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67