Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 
30 /* The switching values for the Throttle Curve Mode switch */
31 #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB)
32 
33 /* Initialize the throttle curves from the airframe file */
35  .nb_curves = THROTTLE_CURVES_NB,
36  .curves = THROTTLE_CURVES
37 };
38 
43 {
44  throttle_curve.mode = THROTTLE_CURVE_MODE_INIT;
45  throttle_curve.throttle = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0];
46  throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0];
47 }
48 
53 void throttle_curve_run(bool motors_on, pprz_t in_cmd[])
54 {
55  // Calculate the mode value from the switch
56  int8_t mode = ((float)(in_cmd[COMMAND_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
57  Bound(mode, 0, THROTTLE_CURVES_NB - 1);
58  throttle_curve.mode = mode;
59 
60  // Check if we have multiple points or a single point
61  struct curve_t curve = throttle_curve.curves[mode];
62  if (curve.nb_points == 1) {
63  throttle_curve.throttle = curve.throttle[0];
64  throttle_curve.collective = curve.collective[0];
65  } else {
66  // Calculate the left point on the curve we need to use
67  uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
68  int8_t curve_p = ((float)in_cmd[COMMAND_THRUST] / curve_range);
69  Bound(curve_p, 0, curve.nb_points - 1);
70 
71  // Calculate the throttle and pitch value
72  uint16_t x = in_cmd[COMMAND_THRUST] - curve_p * curve_range;
73  throttle_curve.throttle = curve.throttle[curve_p]
74  + ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
75  throttle_curve.collective = curve.collective[curve_p]
76  + ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
77  }
78 
79  // Only set throttle if motors are on
80  if (!motors_on) {
81  throttle_curve.throttle = 0;
82  }
83 }
84 
89 {
90  commands[COMMAND_FMODE] = mode * THROTTLE_CURVE_SWITCH_VAL - MAX_PPRZ;
91 }
unsigned short uint16_t
Definition: types.h:16
struct curve_t curves[THROTTLE_CURVES_NB]
Throttle/pitch curves.
int16_t pprz_t
Definition: paparazzi.h:6
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
uint8_t nb_points
The number of points in the curve.
struct throttle_curve_t throttle_curve
uint8_t mode
Flight mode.
uint16_t throttle
Output thrust(throttle) of the throttle curve.
void throttle_curve_run(bool motors_on, pprz_t in_cmd[])
Run the throttle curve and generate the output throttle and pitch This depends on the FMODE(flight mo...
Hardware independent code for commands handling.
int16_t collective[THROTTLE_POINTS_NB]
The collective points in the curve.
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
void throttle_curve_init(void)
Initialize the default throttle curve values.
uint8_t nb_curves
The number of throttle/pitch curves.
#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.
int16_t collective
Output collective of the throttle curve.