Paparazzi UAS  v5.15_devel-113-g1b57ff1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
throttle_curve.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 C. De Wagter
3  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
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  */
27 #include "throttle_curve.h"
28 #include "subsystems/commands.h"
29 #include "autopilot.h"
31 #include "subsystems/abi.h"
32 
33 /* The switching values for the Throttle Curve Mode switch */
34 #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB)
35 
36 /* Default RPM feedback gains and limits */
37 #ifndef THROTTLE_CURVE_RPM_FB_P
38 #define THROTTLE_CURVE_RPM_FB_P 0.0
39 #endif
40 
41 #ifndef THROTTLE_CURVE_RPM_FB_I
42 #define THROTTLE_CURVE_RPM_FB_I 0.0
43 #endif
44 
45 #ifndef THROTTLE_CURVE_RPM_INC_LIMIT
46 #define THROTTLE_CURVE_RPM_INC_LIMIT 512
47 #endif
48 
49 /* Register the RPM callback */
50 #ifndef THROTTLE_CURVE_RPM_ID
51 #define THROTTLE_CURVE_RPM_ID ABI_BROADCAST
52 #endif
53 #ifndef THROTTLE_CURVE_RPM_ACT
54 #define THROTTLE_CURVE_RPM_ACT 0
55 #endif
57 static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
58 
59 /* Initialize the throttle curves from the airframe file */
61  .nb_curves = THROTTLE_CURVES_NB,
62  .curves = THROTTLE_CURVES
63 };
64 
65 #if PERIODIC_TELEMETRY
67 
68 static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev)
69 {
70  pprz_msg_send_THROTTLE_CURVE(trans, dev, AC_ID, &throttle_curve.mode, &throttle_curve.throttle,
71  &throttle_curve.collective,
72  &throttle_curve.rpm, &throttle_curve.rpm_meas, &throttle_curve.rpm_err_sum);
73 }
74 #endif
75 
80 {
81  throttle_curve.mode = THROTTLE_CURVE_MODE_INIT;
82  throttle_curve.nav_mode = THROTTLE_CURVE_MODE_INIT;
83  throttle_curve.throttle = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0];
84  throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0];
85  throttle_curve.rpm_fb_p = THROTTLE_CURVE_RPM_FB_P;
86  throttle_curve.rpm_fb_i = THROTTLE_CURVE_RPM_FB_I;
87  throttle_curve.rpm_err_sum = 0;
88  throttle_curve.rpm_measured = false;
89  throttle_curve.throttle_trim = 0;
90  throttle_curve.coll_trim = 0;
91 
92  AbiBindMsgRPM(THROTTLE_CURVE_RPM_ID, &rpm_ev, rpm_cb);
93 
94 #if PERIODIC_TELEMETRY
96 #endif
97 }
98 
102 static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act)
103 {
104  if(num_act <= THROTTLE_CURVE_RPM_ACT)
105  return;
106 
107  throttle_curve.rpm_meas = rpm[THROTTLE_CURVE_RPM_ACT];
108  throttle_curve.rpm_measured = true;
109 }
110 
116 {
117  // Calculate the mode value from the switch
118  if (ap_mode != AP_MODE_NAV) {
119  int8_t mode = ((float)(radio_control.values[RADIO_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
120  Bound(mode, 0, THROTTLE_CURVES_NB - 1);
121  throttle_curve.mode = mode;
122  } else {
123  throttle_curve.mode = throttle_curve.nav_mode;
124  }
125 
126  // Failsafe curve
127  if (ap_mode == AP_MODE_FAILSAFE) {
128  throttle_curve.mode = 0;
129  }
130 
131  // Check if we have multiple points or a single point
132  struct curve_t curve = throttle_curve.curves[throttle_curve.mode];
133  if (curve.nb_points == 1) {
134  throttle_curve.throttle = curve.throttle[0];
135  throttle_curve.collective = curve.collective[0];
136  throttle_curve.rpm = curve.rpm[0];
137  } else {
138  // Calculate the left point on the curve we need to use
139  uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
140  int8_t curve_p = ((float)cmds[COMMAND_THRUST] / curve_range);
141  Bound(curve_p, 0, curve.nb_points - 1);
142 
143  // Calculate the throttle, pitch and rpm value
144  uint16_t x = cmds[COMMAND_THRUST] - curve_p * curve_range;
145  throttle_curve.throttle = curve.throttle[curve_p]
146  + ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
147  throttle_curve.collective = curve.collective[curve_p]
148  + ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
149  if (curve.rpm[0] != 0xFFFF) {
150  if (throttle_curve.rpm == 0xFFFF) {
151  throttle_curve.rpm = throttle_curve.rpm_meas;
152  }
153  uint16_t new_rpm = curve.rpm[curve_p]
154  + ((curve.rpm[curve_p + 1] - curve.rpm[curve_p]) * x / curve_range);
155  int32_t rpm_diff = new_rpm - throttle_curve.rpm;
156  Bound(rpm_diff, -(THROTTLE_CURVE_RPM_INC_LIMIT/512), THROTTLE_CURVE_RPM_INC_LIMIT/512);
157  throttle_curve.rpm += rpm_diff;
158  } else {
159  throttle_curve.rpm = 0xFFFF;
160  }
161  }
162 
163  // Trim
164  int32_t trimmed_throttle = throttle_curve.throttle + throttle_curve.throttle_trim;
165  Bound(trimmed_throttle, 0, MAX_PPRZ);
166  throttle_curve.throttle = trimmed_throttle;
167 
168  int32_t trimmed_collective = throttle_curve.collective + throttle_curve.coll_trim;
169  Bound(trimmed_collective, -MAX_PPRZ, MAX_PPRZ);
170  throttle_curve.collective = trimmed_collective;
171 
172  // Update RPM feedback
173  if (curve.rpm[0] != 0xFFFF && throttle_curve.rpm_measured) {
174  // Calculate RPM error
175  int32_t rpm_err = (throttle_curve.rpm - throttle_curve.rpm_meas);
176 
177  // Calculate integrated error
178  throttle_curve.rpm_err_sum += rpm_err * throttle_curve.rpm_fb_i / 512.0f;
179  Bound(throttle_curve.rpm_err_sum, -throttle_curve.throttle, (MAX_PPRZ - throttle_curve.throttle));
180 
181  // Calculate feedback command
182  int32_t rpm_feedback = rpm_err * throttle_curve.rpm_fb_p + throttle_curve.rpm_err_sum;
183  Bound(rpm_feedback, -MAX_PPRZ, MAX_PPRZ);
184 
185  // Apply feedback command
186  int32_t new_throttle = throttle_curve.throttle + rpm_feedback;
187  Bound(new_throttle, 0, MAX_PPRZ);
188  throttle_curve.throttle = new_throttle;
189  throttle_curve.rpm_measured = false;
190  } else if (curve.rpm[0] == 0xFFFF) {
191  throttle_curve.rpm_err_sum = 0;
192  }
193 
194  // Set the commands
195  cmds[COMMAND_THRUST] = throttle_curve.throttle; //Reuse for now
196  cmds[COMMAND_COLLECTIVE] = throttle_curve.collective;
197 
198  // Only set throttle if motors are on
199  if (!autopilot_get_motors_on()) {
200  cmds[COMMAND_THRUST] = 0;
201  throttle_curve.rpm_err_sum = 0;
202  }
203 }
204 
209 {
210  int16_t new_mode = mode;
211  Bound(new_mode, 0, THROTTLE_CURVES_NB - 1);
212  throttle_curve.nav_mode = new_mode;
213 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
unsigned short uint16_t
Definition: types.h:16
struct curve_t curves[THROTTLE_CURVES_NB]
Throttle/pitch curves.
#define THROTTLE_CURVE_RPM_FB_P
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act)
RPM callback for RPM based control throttle curves.
float rpm_err_sum
Summed RPM error.
Periodic telemetry system header (includes downlink utility and generated code).
int32_t throttle_trim
RPM feedback i gain.
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:212
float rpm_fb_p
RPM feedback p gain.
int16_t pprz_t
Definition: paparazzi.h:6
Main include for ABI (AirBorneInterface).
void nav_throttle_curve_set(uint8_t mode)
Set a specific throttle curve based on the mode given with this function.
#define THROTTLE_CURVE_SWITCH_VAL
uint16_t rpm_meas
RPM measured.
uint8_t nb_points
The number of points in the curve.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
uint8_t nav_mode
Nav Flight mode.
#define THROTTLE_CURVE_RPM_ID
static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev)
#define THROTTLE_CURVE_RPM_FB_I
static uint8_t cmds[6]
Definition: baro_MS5534A.c:82
struct throttle_curve_t throttle_curve
uint8_t mode
Flight mode.
#define THROTTLE_CURVE_RPM_INC_LIMIT
int32_t coll_trim
Collective trim.
uint16_t rpm
Definition: rpm_sensor.c:33
uint16_t throttle
Output thrust(throttle) of the throttle curve.
signed short int16_t
Definition: types.h:17
#define THROTTLE_CURVE_RPM_ACT
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
int16_t collective[THROTTLE_POINTS_NB]
The collective points in the curve.
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Core autopilot interface common to all firmwares.
#define AP_MODE_NAV
unsigned char uint8_t
Definition: types.h:14
void throttle_curve_init(void)
Initialize the default throttle curve values.
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:69
bool rpm_measured
Whenever the RPM is measured.
void throttle_curve_run(pprz_t cmds[], uint8_t ap_mode)
Run the throttle curve and generate the output throttle and pitch This depends on the FMODE(flight mo...
uint8_t nb_curves
The number of throttle/pitch curves.
uint16_t rpm[THROTTLE_POINTS_NB]
RPM points in the curve.
#define MAX_PPRZ
Definition: paparazzi.h:8
signed char int8_t
Definition: types.h:15
uint16_t throttle[THROTTLE_POINTS_NB]
Throttle points in the curve.
uint16_t rpm
Output RPM of the throttle curve.
#define AP_MODE_FAILSAFE
static abi_event rpm_ev
int16_t collective
Output collective of the throttle curve.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46