Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-19 18:42:46 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[helicopter] Add helicopter mixings and throttle curves

Browse Source
This commit is contained in:
Freek van Tienen
2015-11-05 20:31:28 +01:00
parent 0eb55b6c0d
commit 18f718907e
11 changed files with 614 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDelft/airframes/heli450.xml
+221
View File
@@ -0,0 +1,221 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<!-- this is a quadrotor frame in X-configuration equiped with
* Autopilot: Lisa/MX 2.1 with STM32F4 http://wiki.paparazziuav.org/wiki/Lisa/M_v21
* IMU: Integrated Aspirin 2.2 http://wiki.paparazziuav.org/wiki/AspirinIMU
* Actuators: Asctec motor controllers http://wiki.paparazziuav.org/wiki/Subsystem/actuators#Asctec_v2
* GPS: Piksi http://wiki.paparazziuav.org/wiki/Subsystem/gps
* RC: one Spektrum sats http://wiki.paparazziuav.org/wiki/Subsystem/radio_control#Spektrum
-->
<airframe name="heli450">
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0"/>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
</target>
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_MODE" value="RADIO_GEAR"/>
</subsystem>
<subsystem name="actuators" type="pwm"/>
<subsystem name="telemetry" type="xbee_api"/>
<subsystem name="imu" type="aspirin_v2.2"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
</firmware>
<modules>
<load name="geo_mag.xml"/>
<load name="air_data.xml"/>
<load name="gps_ubx_ucenter.xml"/>
<load name="heli_swashplate_mixing.xml"/>
<load name="heli_throttle_curve.xml"/>
</modules>
<servos driver="Pwm">
<servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="FRONT" no="1" min="2100" neutral="1500" max="900"/>
<servo name="RIGHTBACK" no="2" min="900" neutral="1500" max="2100"/>
<servo name="LEFTBACK" no="3" min="2100" neutral="1500" max="900"/>
<servo name="TAIL" no="4" min="1100" neutral="1450" max="1800"/>
</servos>
<commands>
<axis name="THRUST" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="FMODE" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
<set command="FMODE" value="@AUX2"/>
</rc_commands>
<section name="MIXING" prefix="SW_MIXING_">
<!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
<define name="TYPE" value="H120"/>
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_COLL" value="0"/>
</section>
<command_laws>
<call fun="throttle_curve_run(autopilot_motors_on, values)"/>
<call fun="swashplate_mixing_run(values)"/>
<set servo="THROTTLE" value="throttle_curve.throttle"/>
<set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>
<set servo="RIGHTBACK" value="swashplate_mixing.commands[SW_RIGHTBACK]"/>
<set servo="LEFTBACK" value="swashplate_mixing.commands[SW_LEFTBACK]"/>
<set servo="TAIL" value="@YAW + throttle_curve.throttle*2800/7000"/>
</command_laws>
<heli_curves>
<curve throttle="0,4800,7200,8400,9600" collective="-1000,750,2500,4250,6000"/>
<curve throttle="9600,7200,9600" collective="-7500,0,7500"/>
</heli_curves>
<section name="MISC">
<define name="NAV_CLIMB_VSPEED" value="2.5"/>
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="11"/>
<define name="ACCEL_Y_NEUTRAL" value="11"/>
<define name="ACCEL_Z_NEUTRAL" value="-25"/>
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="14"/>
<define name="MAG_Y_NEUTRAL" value="116"/>
<define name="MAG_Z_NEUTRAL" value="119"/>
<define name="MAG_X_SENS" value="5.09245681612" integer="16"/>
<define name="MAG_Y_SENS" value="5.29702744632" integer="16"/>
<define name="MAG_Z_SENS" value="5.65287938992" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="-90." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="65." unit="deg"/>
<define name="SP_MAX_THETA" value="65." unit="deg"/>
<define name="SP_MAX_R" value="250." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
<define name="PSI_DDGAIN" value=" 200"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="35" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_x_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/TUDelft/conf.xml
+11
View File
@@ -87,6 +87,17 @@
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
gui_color="red"
/>
<aircraft
name="Heli450"
ac_id="99"
airframe="airframes/TUDelft/airframes/heli450.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
<aircraft
name="Helisa"
ac_id="114"
conf/airframes/airframe.dtd
+9 -1
View File
@@ -1,6 +1,6 @@
<!-- Paparazzi airframe DTD -->
<!ELEMENT airframe (include|servos|commands|rc_commands|auto_rc_commands|ap_only_commands|command_laws|section|makefile|modules|firmware|autopilot)*>
<!ELEMENT airframe (include|servos|commands|rc_commands|auto_rc_commands|ap_only_commands|command_laws|section|makefile|modules|firmware|autopilot|heli_curves)*>
<!ELEMENT include EMPTY>
<!ELEMENT servos (servo)*>
<!ELEMENT commands (axis)*>
@@ -8,6 +8,7 @@
<!ELEMENT auto_rc_commands (set)*>
<!ELEMENT ap_only_commands (copy)*>
<!ELEMENT command_laws (let|set|call|ratelimit)*>
<!ELEMENT heli_curves (curve)*>
<!ELEMENT section (define|linear)*>
<!ELEMENT servo EMPTY>
<!ELEMENT axis EMPTY>
@@ -15,6 +16,7 @@
<!ELEMENT call EMPTY>
<!ELEMENT ratelimit EMPTY>
<!ELEMENT copy EMPTY>
<!ELEMENT curve EMPTY>
<!ELEMENT let EMPTY>
<!ELEMENT define EMPTY>
<!ELEMENT linear EMPTY>
@@ -57,6 +59,8 @@ driver CDATA #IMPLIED>
<!ATTLIST ap_only_commands>
<!ATTLIST command_laws>
<!ATTLIST heli_curves>
<!ATTLIST section
name CDATA #IMPLIED
prefix CDATA #IMPLIED>
@@ -68,6 +72,10 @@ min CDATA #REQUIRED
neutral CDATA #REQUIRED
max CDATA #REQUIRED>
<!ATTLIST curve
throttle CDATA #REQUIRED
collective CDATA #REQUIRED>
<!ATTLIST axis
name CDATA #REQUIRED
failsafe_value CDATA #REQUIRED>
conf/modules/heli_swashplate_mixing.xml
+17
View File
@@ -0,0 +1,17 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="swashplate_mixing" dir="helicopter">
<doc>
<description>Helicopter Swashplate Mixing</description>
</doc>
<depends>heli_throttle_curve.xml</depends>
<header>
<file name="swashplate_mixing.h"/>
</header>
<init fun="swashplate_mixing_init()"/>
<makefile>
<file name="swashplate_mixing.c"/>
<define name="ROTORCRAFT_IS_HELI" value="TRUE"/>
</makefile>
</module>
conf/modules/heli_throttle_curve.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="throttle_curve" dir="helicopter">
<doc>
<description>Throttle Curve Mixers</description>
</doc>
<header>
<file name="throttle_curve.h"/>
</header>
<init fun="throttle_curve_init()"/>
<makefile>
<file name="throttle_curve.c"/>
</makefile>
</module>
sw/airborne/firmwares/rotorcraft/autopilot.h
+10
View File
@@ -111,6 +111,15 @@ extern uint16_t autopilot_flight_time;
* Limit thrust and/or yaw depending of the in_flight
* and motors_on flag status
*/
#ifdef ROTORCRAFT_IS_HELI
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#else
#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
@@ -129,6 +138,7 @@ extern uint16_t autopilot_flight_time;
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#endif
#endif
/** Z-acceleration threshold to detect ground in m/s^2 */
#ifndef THRESHOLD_GROUND_DETECT
sw/airborne/modules/helicopter/swashplate_mixing.c
+91
View File
@@ -0,0 +1,91 @@
/*
* Copyright (C) 2015 C. De Wagter
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/helicopter/swashplate_mixing.c"
* @author C. De Wagter and Freek van Tienen
* Helicopter Swashplate Mixing
*/
#include "swashplate_mixing.h"
#include "throttle_curve.h"
PRINT_CONFIG_VAR(SW_MIXING_TYPE)
struct swashplate_mixing_t swashplate_mixing;
/* Coeficients per motor */
static const float roll_coef[SW_NB] = SW_MIXING_ROLL_COEF;
static const float pitch_coef[SW_NB] = SW_MIXING_PITCH_COEF;
static const float coll_coef[SW_NB] = SW_MIXING_COLL_COEF;
/* Default roll trim */
#ifndef SW_MIXING_TRIM_ROLL
#define SW_MIXING_TRIM_ROLL 0
#endif
PRINT_CONFIG_VAR(SW_MIXING_TRIM_ROLL)
/* Default pitch trim */
#ifndef SW_MIXING_TRIM_PITCH
#define SW_MIXING_TRIM_PITCH 0
#endif
PRINT_CONFIG_VAR(SW_MIXING_TRIM_PITCH)
/* Default collective trim */
#ifndef SW_MIXING_TRIM_COLL
#define SW_MIXING_TRIM_COLL 0
#endif
PRINT_CONFIG_VAR(SW_MIXING_TRIM_COLL)
/**
* Initialize the motor mixing and calculate the trim values
*/
void swashplate_mixing_init()
{
uint8_t i;
// Go trough all the motors and calculate the trim value and set the initial command
for (i = 0; i < SW_NB; i++) {
swashplate_mixing.commands[i] = 0;
swashplate_mixing.trim[i] =
roll_coef[i] * SW_MIXING_TRIM_ROLL +
pitch_coef[i] * SW_MIXING_TRIM_PITCH +
coll_coef[i] * SW_MIXING_TRIM_COLL;
}
}
/*
* Run the swashplate mixing
* This depends on the ROLL and PITCH command
* It also depends on the throttle_curve.collective
*/
void swashplate_mixing_run(pprz_t in_cmd[])
{
uint8_t i;
// Go trough all the motors and calculate the command
for (i = 0; i < SW_NB; i++) {
swashplate_mixing.commands[i] = swashplate_mixing.trim[i] +
roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
coll_coef[i] * throttle_curve.collective;
BoundAbs(swashplate_mixing.commands[i], MAX_PPRZ);
}
}
sw/airborne/modules/helicopter/swashplate_mixing.h
+81
View File
@@ -0,0 +1,81 @@
/*
* Copyright (C) 2015 C. De Wagter
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/helicopter/swashplate_mixing.h"
* @author C. De Wagter and Freek van Tienen
* Helicopter Swashplate Mixing
*/
#ifndef SWASHPLATE_MIXING_H
#define SWASHPLATE_MIXING_H
#include "std.h"
#include "paparazzi.h"
#include "generated/airframe.h"
/* Different type of swashplate mixings */
#define MECH 0
#define H120 1
#define HR120 2
/**
* MECH (front/right/coll), H120 (front/rightback/leftback), HR120 (back/leftfront/rightfront)
*/
#if SW_MIXING_TYPE == MECH
#define SW_NB 3
#define SW_FRONT 0
#define SW_RIGHT 1
#define SW_COLL 2
#define SW_MIXING_ROLL_COEF { 0, -1, 0 }
#define SW_MIXING_PITCH_COEF { 1, 0, 0 }
#define SW_MIXING_COLL_COEF { 0, 0, 1 }
#elif SW_MIXING_TYPE == H120
#define SW_NB 3
#define SW_FRONT 0
#define SW_RIGHTBACK 1
#define SW_LEFTBACK 2
#define SW_MIXING_ROLL_COEF { 0, -0.866, 0.866 }
#define SW_MIXING_PITCH_COEF { 1, -0.5, -0.5 }
#define SW_MIXING_COLL_COEF { 1, 1, 1 }
#elif SW_MIXING_TYPE == HR120
#define SW_NB 3
#define SW_BACK 0
#define SW_LEFTFRONT 1
#define SW_RIGHTFRONT 2
#define SW_MIXING_ROLL_COEF { 0, 0.866, -0.866 }
#define SW_MIXING_PITCH_COEF { -1, 0.5, 0.5 }
#define SW_MIXING_COLL_COEF { 1, 1, 1 }
#endif
/* Swashplate mixing structure */
struct swashplate_mixing_t {
int32_t commands[SW_NB]; ///< The output commands
int32_t trim[SW_NB]; ///< Trim values for the different actuators
};
extern struct swashplate_mixing_t swashplate_mixing;
/* External used functions */
extern void swashplate_mixing_init(void);
extern void swashplate_mixing_run(pprz_t in_cmd[]);
#endif
sw/airborne/modules/helicopter/throttle_curve.c
+82
View File
@@ -0,0 +1,82 @@
/*
* Copyright (C) 2015 C. De Wagter
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/helicopter/throttle_curve.c"
* @author C. De Wagter and Freek van Tienen
* Throttle Curve Mixers
*/
#include "throttle_curve.h"
/* The switching values for the Throttle Curve Mode switch */
#define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB)
/* Initialize the throttle curves from the airframe file */
struct throttle_curve_t throttle_curve = {
.nb_curves = THROTTLE_CURVES_NB,
.curves = THROTTLE_CURVES
};
/**
* Initialize the default throttle curve values
*/
void throttle_curve_init(void)
{
throttle_curve.mode = THROTTLE_CURVE_MODE_INIT;
throttle_curve.throttle = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0];
throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0];
}
/**
* Run the throttle curve and generate the output throttle and pitch
* This depends on the FMODE(flight mode) and TRHUST command
*/
void throttle_curve_run(bool_t motors_on, pprz_t in_cmd[])
{
// Calculate the mode value from the switch
int8_t mode = ((float)(in_cmd[COMMAND_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
Bound(mode, 0, THROTTLE_CURVES_NB - 1);
throttle_curve.mode = mode;
// Check if we have multiple points or a single point
struct curve_t curve = throttle_curve.curves[mode];
if (curve.nb_points == 1) {
throttle_curve.throttle = curve.throttle[0];
throttle_curve.collective = curve.collective[0];
} else {
// Calculate the left point on the curve we need to use
uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
int8_t curve_p = ((float)in_cmd[COMMAND_THRUST] / curve_range);
Bound(curve_p, 0, curve.nb_points - 1);
// Calculate the throttle and pitch value
uint16_t x = in_cmd[COMMAND_THRUST] - curve_p * curve_range;
throttle_curve.throttle = curve.throttle[curve_p]
+ ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
throttle_curve.collective = curve.collective[curve_p]
+ ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
}
// Only set throttle if motors are on
if (!motors_on) {
throttle_curve.throttle = 0;
}
}
sw/airborne/modules/helicopter/throttle_curve.h
+56
View File
@@ -0,0 +1,56 @@
/*
* Copyright (C) 2015 C. De Wagter
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/helicopter/throttle_curve.h"
* @author C. De Wagter and Freek van Tienen
* Throttle Curve Mixers
*/
#ifndef THROTTLE_CURVE_H
#define THROTTLE_CURVE_H
#include "std.h"
#include "paparazzi.h"
#include "generated/airframe.h"
/* Throttle and collective curve */
struct curve_t {
uint8_t nb_points; ///< The number of points in the curve
uint16_t throttle[THROTTLE_POINTS_NB]; ///< Throttle points in the curve
int16_t collective[THROTTLE_POINTS_NB]; ///< The collective points in the curve
};
/* Main throttle curve structure */
struct throttle_curve_t {
uint8_t mode; ///< Flight mode
uint8_t nb_curves; ///< The number of throttle/pitch curves
struct curve_t curves[THROTTLE_CURVES_NB]; ///< Throttle/pitch curves
uint16_t throttle; ///< Output thrust(throttle) of the throttle curve
int16_t collective; ///< Output collective of the throttle curve
};
extern struct throttle_curve_t throttle_curve;
/* External functions */
extern void throttle_curve_init(void);
void throttle_curve_run(bool_t motors_on, pprz_t in_cmd[]);
#endif
sw/tools/generators/gen_airframe.ml
+21
View File
@@ -262,6 +262,17 @@ let parse_command = fun command no ->
let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
{ failsafe_value = failsafe_value; foo = 0}
let parse_heli_curves = fun heli_surves ->
let a = fun s -> ExtXml.attrib heli_surves s in
match Xml.tag heli_surves with
"curve" ->
let throttle = a "throttle" in
let collective = a "collective" in
printf " {.nb_points = %i, \\\n" (List.length (Str.split (Str.regexp ",") throttle));
printf " .throttle = {%s}, \\\n" throttle;
printf " .collective = {%s}}, \\\n" collective
| _ -> xml_error "mixer"
let rec parse_section = fun ac_id s ->
match Xml.tag s with
"section" ->
@@ -315,6 +326,16 @@ let rec parse_section = fun ac_id s ->
List.iter parse_command_laws (Xml.children s);
printf " AllActuatorsCommit(); \\\n";
printf "}\n\n";
| "heli_curves" ->
let default = ExtXml.attrib_or_default s "default" "0" in
let curves = Xml.children s in
let nb_points = List.fold_right (fun s m -> Pervasives.max (List.length (Str.split (Str.regexp ",") (ExtXml.attrib s "throttle"))) m) curves 0 in
define "THROTTLE_CURVE_MODE_INIT" default;
define "THROTTLE_CURVES_NB" (string_of_int (List.length curves));
define "THROTTLE_POINTS_NB" (string_of_int nb_points);
printf "#define THROTTLE_CURVES { \\\n";
List.iter parse_heli_curves curves;
printf "}\n\n";
| "include" ->
let filename = Str.global_replace (Str.regexp "\\$AC_ID") ac_id (ExtXml.attrib s "href") in
let subxml = Xml.parse_file filename in