Paparazzi UAS v7.0_unstable
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"
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
63
66
67/* Initialize the throttle curves from the airframe file */
72
73#if PERIODIC_TELEMETRY
75
82#endif
83
106
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) {
132 } else {
134 }
135
136 // Failsafe curve
137 if (ap_mode == AP_MODE_FAILSAFE) {
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];
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));
151 Bound(curve_p, 0, curve.nb_points - 1);
152
153 // Calculate the throttle, pitch and rpm value
156 + ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
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 }
164 + ((curve.rpm[curve_p + 1] - curve.rpm[curve_p]) * x / curve_range);
168 } else {
169 throttle_curve.rpm = 0xFFFF;
170 }
171 }
172
173 // Trim
177
181
182 // Update RPM feedback
183 if (curve.rpm[0] != 0xFFFF && throttle_curve.rpm_measured) {
184 // Calculate RPM error
186
187 // Calculate integrated error
190
191 // Calculate feedback command
194
195 // Apply feedback command
200 } else if (curve.rpm[0] == 0xFFFF) {
202 }
203
204 // Set the commands
205 cmds[COMMAND_THRUST] = throttle_curve.throttle; //Reuse for now
207
208 // Only set throttle if motors are on
210 cmds[COMMAND_THRUST] = 0;
212 }
213}
214
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]
#define UNUSED(x)
Hardware independent code for commands handling.
uint16_t foo
Definition main_demo5.c:58
int32_t rpm
RPM.
Definition actuators.h:51
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
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#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
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.
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.
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.
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.