Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/commands.h"
29 #include "autopilot.h"
31 #include "modules/core/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 PRINT_CONFIG_VAR(THROTTLE_CURVE_RPM_FB_P)
41 
42 #ifndef THROTTLE_CURVE_RPM_FB_I
43 #define THROTTLE_CURVE_RPM_FB_I 0.0
44 #endif
45 PRINT_CONFIG_VAR(THROTTLE_CURVE_RPM_FB_I)
46 
47 #ifndef THROTTLE_CURVE_RPM_INC_LIMIT
48 #define THROTTLE_CURVE_RPM_INC_LIMIT 512
49 #endif
50 PRINT_CONFIG_VAR(THROTTLE_CURVE_RPM_INC_LIMIT)
51 
52 /* Register the RPM callback */
53 #ifndef THROTTLE_CURVE_ACT_FEEDBACK_ID
54 #define THROTTLE_CURVE_ACT_FEEDBACK_ID ABI_BROADCAST
55 #endif
56 PRINT_CONFIG_VAR(THROTTLE_CURVE_ACT_FEEDBACK_ID)
57 
58 /* Which actuator to listen for RPM feedback */
59 #ifndef THROTTLE_CURVE_RPM_ACT
60 #error "There needs to be an actuator to listen for RPM feedback, please set THROTTLE_CURVE_RPM_ACT"
61 #endif
62 PRINT_CONFIG_VAR(THROTTLE_CURVE_RPM_ACT)
63 
65 static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
66 
67 /* Initialize the throttle curves from the airframe file */
69  .nb_curves = THROTTLE_CURVES_NB,
70  .curves = THROTTLE_CURVES
71 };
72 
73 #if PERIODIC_TELEMETRY
75 
76 static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev)
77 {
78  pprz_msg_send_THROTTLE_CURVE(trans, dev, AC_ID, &throttle_curve.mode, &throttle_curve.throttle,
81 }
82 #endif
83 
88 {
89  throttle_curve.mode = THROTTLE_CURVE_MODE_INIT;
90  throttle_curve.nav_mode = THROTTLE_CURVE_MODE_INIT;
91  throttle_curve.throttle = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0];
92  throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0];
99 
101 
102 #if PERIODIC_TELEMETRY
104 #endif
105 }
106 
110 static void act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
111 {
112  // Search for the actuator
113  for(uint8_t i = 0; i < num_act; i++) {
114  if(feedback[i].idx == THROTTLE_CURVE_RPM_ACT && feedback[i].set.rpm) {
115  throttle_curve.rpm_meas = feedback[i].rpm;
117  }
118  }
119 }
120 
126 {
127  // Calculate the mode value from the switch
128  if (ap_mode != AP_MODE_NAV) {
129  int8_t mode = ((float)(radio_control.values[RADIO_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
130  Bound(mode, 0, THROTTLE_CURVES_NB - 1);
132  } else {
134  }
135 
136  // Failsafe curve
137  if (ap_mode == AP_MODE_FAILSAFE) {
138  throttle_curve.mode = 0;
139  }
140 
141  // Check if we have multiple points or a single point
143  if (curve.nb_points == 1) {
144  throttle_curve.throttle = curve.throttle[0];
146  throttle_curve.rpm = curve.rpm[0];
147  } else {
148  // Calculate the left point on the curve we need to use
149  uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
150  int8_t curve_p = ((float)cmds[COMMAND_THRUST] / curve_range);
151  Bound(curve_p, 0, curve.nb_points - 1);
152 
153  // Calculate the throttle, pitch and rpm value
154  uint16_t x = cmds[COMMAND_THRUST] - curve_p * curve_range;
155  throttle_curve.throttle = curve.throttle[curve_p]
156  + ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
157  throttle_curve.collective = curve.collective[curve_p]
158  + ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
159  if (curve.rpm[0] != 0xFFFF) {
160  if (throttle_curve.rpm == 0xFFFF) {
162  }
163  uint16_t new_rpm = curve.rpm[curve_p]
164  + ((curve.rpm[curve_p + 1] - curve.rpm[curve_p]) * x / curve_range);
165  int32_t rpm_diff = new_rpm - throttle_curve.rpm;
166  Bound(rpm_diff, -(THROTTLE_CURVE_RPM_INC_LIMIT/512), THROTTLE_CURVE_RPM_INC_LIMIT/512);
167  throttle_curve.rpm += rpm_diff;
168  } else {
169  throttle_curve.rpm = 0xFFFF;
170  }
171  }
172 
173  // Trim
175  Bound(trimmed_throttle, 0, MAX_PPRZ);
176  throttle_curve.throttle = trimmed_throttle;
177 
179  Bound(trimmed_collective, -MAX_PPRZ, MAX_PPRZ);
180  throttle_curve.collective = trimmed_collective;
181 
182  // Update RPM feedback
183  if (curve.rpm[0] != 0xFFFF && throttle_curve.rpm_measured) {
184  // Calculate RPM error
186 
187  // Calculate integrated error
188  throttle_curve.rpm_err_sum += rpm_err * throttle_curve.rpm_fb_i / 512.0f;
190 
191  // Calculate feedback command
192  int32_t rpm_feedback = rpm_err * throttle_curve.rpm_fb_p + throttle_curve.rpm_err_sum;
193  Bound(rpm_feedback, -MAX_PPRZ, MAX_PPRZ);
194 
195  // Apply feedback command
196  int32_t new_throttle = throttle_curve.throttle + rpm_feedback;
197  Bound(new_throttle, 0, MAX_PPRZ);
198  throttle_curve.throttle = new_throttle;
200  } else if (curve.rpm[0] == 0xFFFF) {
202  }
203 
204  // Set the commands
205  cmds[COMMAND_THRUST] = throttle_curve.throttle; //Reuse for now
206  cmds[COMMAND_COLLECTIVE] = throttle_curve.collective;
207 
208  // Only set throttle if motors are on
209  if (!autopilot_get_motors_on()) {
210  cmds[COMMAND_THRUST] = 0;
212  }
213 }
214 
219 {
220  int16_t new_mode = mode;
221  Bound(new_mode, 0, THROTTLE_CURVES_NB - 1);
222  throttle_curve.nav_mode = new_mode;
223 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:295
Core autopilot interface common to all firmwares.
static uint8_t cmds[6]
Definition: baro_MS5534A.c:82
Hardware independent code for commands handling.
uint8_t last_wp UNUSED
int32_t rpm
RPM.
Definition: actuators.h:51
static uint32_t idx
int16_t pprz_t
Definition: paparazzi.h:6
#define MAX_PPRZ
Definition: paparazzi.h:8
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
#define AP_MODE_NAV
#define AP_MODE_FAILSAFE
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
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
#define THROTTLE_CURVE_SWITCH_VAL
struct throttle_curve_t throttle_curve
#define THROTTLE_CURVE_RPM_FB_P
void throttle_curve_init(void)
Initialize the default throttle curve values.
static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev)
static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
RPM callback for RPM based control throttle curves.
#define THROTTLE_CURVE_ACT_FEEDBACK_ID
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...
#define THROTTLE_CURVE_RPM_INC_LIMIT
void nav_throttle_curve_set(uint8_t mode)
Set a specific throttle curve based on the mode given with this function.
#define THROTTLE_CURVE_RPM_FB_I
static abi_event act_feedback_ev
int32_t coll_trim
Collective trim.
uint8_t mode
Flight mode.
float rpm_fb_p
RPM feedback p gain.
uint16_t throttle[THROTTLE_POINTS_NB]
Throttle points in the curve.
uint16_t rpm
Output RPM of the throttle curve.
uint16_t rpm_meas
RPM measured.
int16_t collective[THROTTLE_POINTS_NB]
The collective points in the curve.
uint16_t rpm[THROTTLE_POINTS_NB]
RPM points in the curve.
uint8_t nb_curves
The number of throttle/pitch curves.
uint8_t nav_mode
Nav Flight mode.
int32_t throttle_trim
RPM feedback i gain.
uint16_t throttle
Output thrust(throttle) of the throttle curve.
struct curve_t curves[THROTTLE_CURVES_NB]
Throttle/pitch curves.
int16_t collective
Output collective of the throttle curve.
uint8_t nb_points
The number of points in the curve.
bool rpm_measured
Whenever the RPM is measured.
float rpm_err_sum
Summed RPM error.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103