Paparazzi UAS  v7.0_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 {
97  // indi init is already done through module init
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 }
float q
in rad/s
float p
in rad/s
float r
in rad/s
angular rates
#define MAX_PPRZ
Definition: paparazzi.h:8
struct RadioControl radio_control
Definition: radio_control.c:33
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
Some helper functions to check RC sticks.
#define THROTTLE_STICK_DOWN()
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:34
General stabilization interface for rotorcrafts.
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
void stabilization_rate_read_rc(void)
Read RC comands with roll and yaw sticks.
#define YAW_RATE_DEADBAND_EXCEEDED()
void stabilization_rate_enter(void)
Reset rate controller.
#define STABILIZATION_INDI_MAX_RATE
Maximum rate you can request in RC rate mode (rad/s)
#define ROLL_RATE_DEADBAND_EXCEEDED()
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 ...
static void send_rate(struct transport_tx *trans, struct link_device *dev)
void stabilization_rate_run(bool in_flight)
Run indi rate interface from the "stabilization_rate_run" function.
#define PITCH_RATE_DEADBAND_EXCEEDED()
struct FloatRates stabilization_rate_sp
void stabilization_rate_init(void)
Initialize rate controller.
Rate stabilization for rotorcrafts based on INDI.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66