Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 */
26
27#include "throttle_curve.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
41
42#ifndef THROTTLE_CURVE_RPM_FB_I
43#define THROTTLE_CURVE_RPM_FB_I 0.0
44#endif
46
47#ifndef THROTTLE_CURVE_RPM_INC_LIMIT
48#define THROTTLE_CURVE_RPM_INC_LIMIT 512
49#endif
51
52/* Register the RPM callback */
53#ifndef THROTTLE_CURVE_ACT_FEEDBACK_ID
54#define THROTTLE_CURVE_ACT_FEEDBACK_ID ABI_BROADCAST
55#endif
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
62PRINT_CONFIG_VAR(THROTTLE_CURVE_RPM_ACT)
63
65static 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
76static 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,
79 &throttle_curve.collective,
80 &throttle_curve.rpm, &throttle_curve.rpm_meas, &throttle_curve.rpm_err_sum);
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];
95 throttle_curve.rpm_err_sum = 0;
96 throttle_curve.rpm_measured = false;
97 throttle_curve.throttle_trim = 0;
98 throttle_curve.coll_trim = 0;
99
101
102#if PERIODIC_TELEMETRY
104#endif
105}
106
110static 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;
116 throttle_curve.rpm_measured = true;
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);
131 throttle_curve.mode = mode;
132 } else {
133 throttle_curve.mode = throttle_curve.nav_mode;
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
142 struct curve_t curve = throttle_curve.curves[throttle_curve.mode];
143 if (curve.nb_points == 1) {
144 throttle_curve.throttle = curve.throttle[0];
145 throttle_curve.collective = curve.collective[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) {
161 throttle_curve.rpm = throttle_curve.rpm_meas;
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;
167 throttle_curve.rpm += rpm_diff;
168 } else {
169 throttle_curve.rpm = 0xFFFF;
170 }
171 }
172
173 // Trim
174 int32_t trimmed_throttle = throttle_curve.throttle + throttle_curve.throttle_trim;
175 Bound(trimmed_throttle, 0, MAX_PPRZ);
176 throttle_curve.throttle = trimmed_throttle;
177
178 int32_t trimmed_collective = throttle_curve.collective + throttle_curve.coll_trim;
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
185 int32_t rpm_err = (throttle_curve.rpm - throttle_curve.rpm_meas);
186
187 // Calculate integrated error
188 throttle_curve.rpm_err_sum += rpm_err * throttle_curve.rpm_fb_i / 512.0f;
189 Bound(throttle_curve.rpm_err_sum, -throttle_curve.throttle, (MAX_PPRZ - throttle_curve.throttle));
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;
199 throttle_curve.rpm_measured = false;
200 } else if (curve.rpm[0] == 0xFFFF) {
201 throttle_curve.rpm_err_sum = 0;
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
210 cmds[COMMAND_THRUST] = 0;
211 throttle_curve.rpm_err_sum = 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).
struct abi_struct abi_event
Definition abi_common.h:101
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
Core autopilot interface common to all firmwares.
static uint8_t cmds[6]
#define UNUSED(x)
Hardware independent code for commands handling.
static struct uart_periph * dev
int32_t rpm
RPM.
Definition actuators.h:54
static uint32_t idx
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
int16_t pprz_t
Definition paparazzi.h:6
#define MAX_PPRZ
Definition paparazzi.h:8
static abi_event act_feedback_ev
struct RadioControl radio_control
Generic interface for radio control modules.
#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:65
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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
uint16_t throttle[THROTTLE_POINTS_NB]
Throttle points in the curve.
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_points
The number of points in the curve.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.