From 7b3988f2a91c1b685c7bae3f67a777919916a094 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sun, 4 Dec 2016 10:32:58 +0100 Subject: [PATCH 1/2] [rpm_control] throttle curves with RPM control --- conf/airframes/airframe.dtd | 1 + conf/airframes/tudelft/outback.xml | 73 +++++---- conf/conf_tests.xml | 24 ++- conf/modules/heli_swashplate_mixing.xml | 1 - conf/modules/heli_throttle_curve.xml | 17 ++- conf/modules/rpm_control.xml | 16 ++ conf/telemetry/tudelft/outback.xml | 1 + conf/userconf/tudelft/conf.xml | 2 +- sw/airborne/firmwares/rotorcraft/main_ap.c | 5 + .../modules/helicopter/throttle_curve.c | 144 ++++++++++++++++-- .../modules/helicopter/throttle_curve.h | 14 +- sw/tools/generators/gen_airframe.ml | 22 ++- 12 files changed, 274 insertions(+), 46 deletions(-) create mode 100644 conf/modules/rpm_control.xml diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd index 7b1e64d9ba..1cd899ed92 100644 --- a/conf/airframes/airframe.dtd +++ b/conf/airframes/airframe.dtd @@ -77,6 +77,7 @@ max CDATA #REQUIRED> - @@ -116,57 +115,79 @@ - - - - - + +
- - + + +
+ +
+ +
- + - + + - + - - - - - - + + + + + + - - + - - - + + + - + + + + + + + + + + + + +
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 036303a1ac..a6c96d2953 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -32,6 +32,17 @@ settings_modules="modules/nav_basic_fw.xml modules/gps.xml modules/ahrs_float_dcm.xml modules/imu_common.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml" gui_color="blue" /> + + /> + Helicopter Swashplate Mixing - heli_throttle_curve.xml
diff --git a/conf/modules/heli_throttle_curve.xml b/conf/modules/heli_throttle_curve.xml index 8660130ef2..1c9326bc9b 100644 --- a/conf/modules/heli_throttle_curve.xml +++ b/conf/modules/heli_throttle_curve.xml @@ -3,13 +3,28 @@ Throttle Curve Mixers + + + + + + + + + + + + + +
- + + diff --git a/conf/modules/rpm_control.xml b/conf/modules/rpm_control.xml new file mode 100644 index 0000000000..14cc307707 --- /dev/null +++ b/conf/modules/rpm_control.xml @@ -0,0 +1,16 @@ + + + + + RPM controller with feedback + +
+ +
+ + + + + + + diff --git a/conf/telemetry/tudelft/outback.xml b/conf/telemetry/tudelft/outback.xml index 476de8c88f..9cf8a17f77 100644 --- a/conf/telemetry/tudelft/outback.xml +++ b/conf/telemetry/tudelft/outback.xml @@ -30,6 +30,7 @@ + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 628a0e00fd..1468d747b4 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -173,7 +173,7 @@ telemetry="telemetry/tudelft/outback.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" + settings_modules="modules/heli_throttle_curve.xml modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="#ffffdffac31f" release="dc8a90d35290784bcc817edff08d8da2df64bcef" /> diff --git a/sw/airborne/firmwares/rotorcraft/main_ap.c b/sw/airborne/firmwares/rotorcraft/main_ap.c index 23d192490e..841ee650e7 100644 --- a/sw/airborne/firmwares/rotorcraft/main_ap.c +++ b/sw/airborne/firmwares/rotorcraft/main_ap.c @@ -247,6 +247,11 @@ void main_periodic(void) autopilot_periodic(); /* set actuators */ //actuators_set(autopilot_get_motors_on()); + +#if USE_THROTTLE_CURVES + throttle_curve_run(commands, autopilot_get_mode()); +#endif + #ifndef INTER_MCU_AP SetActuatorsFromCommands(commands, autopilot_get_mode()); #else diff --git a/sw/airborne/modules/helicopter/throttle_curve.c b/sw/airborne/modules/helicopter/throttle_curve.c index 3f7d24ac3c..04f36627a0 100644 --- a/sw/airborne/modules/helicopter/throttle_curve.c +++ b/sw/airborne/modules/helicopter/throttle_curve.c @@ -26,59 +26,179 @@ #include "throttle_curve.h" #include "subsystems/commands.h" +#include "autopilot.h" +#include "subsystems/radio_control.h" +#include "subsystems/abi.h" /* The switching values for the Throttle Curve Mode switch */ #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB) +/* Default RPM feedback gains and limits */ +#ifndef THROTTLE_CURVE_RPM_FB_P +#define THROTTLE_CURVE_RPM_FB_P 0.0 +#endif + +#ifndef THROTTLE_CURVE_RPM_FB_I +#define THROTTLE_CURVE_RPM_FB_I 0.0 +#endif + +#ifndef THROTTLE_CURVE_RPM_INC_LIMIT +#define THROTTLE_CURVE_RPM_INC_LIMIT 512 +#endif + +/* Register the RPM callback */ +#ifndef THROTTLE_CURVE_RPM_ID +#define THROTTLE_CURVE_RPM_ID ABI_BROADCAST +#endif +#ifndef THROTTLE_CURVE_RPM_ACT +#define THROTTLE_CURVE_RPM_ACT 0 +#endif +static abi_event rpm_ev; +static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act); + /* Initialize the throttle curves from the airframe file */ struct throttle_curve_t throttle_curve = { .nb_curves = THROTTLE_CURVES_NB, .curves = THROTTLE_CURVES }; +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_THROTTLE_CURVE(trans, dev, AC_ID, &throttle_curve.mode, &throttle_curve.throttle, + &throttle_curve.collective, + &throttle_curve.rpm, &throttle_curve.rpm_meas, &throttle_curve.rpm_err_sum); +} +#endif + /** * Initialize the default throttle curve values */ void throttle_curve_init(void) { throttle_curve.mode = THROTTLE_CURVE_MODE_INIT; + throttle_curve.nav_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]; + throttle_curve.rpm_fb_p = THROTTLE_CURVE_RPM_FB_P; + throttle_curve.rpm_fb_i = THROTTLE_CURVE_RPM_FB_I; + throttle_curve.rpm_err_sum = 0; + throttle_curve.rpm_measured = false; + throttle_curve.throttle_trim = 0; + throttle_curve.coll_trim = 0; + + AbiBindMsgRPM(THROTTLE_CURVE_RPM_ID, &rpm_ev, rpm_cb); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_THROTTLE_CURVE, throttle_curve_send_telem); +#endif +} + +/** + * RPM callback for RPM based control throttle curves + */ +static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act) +{ + if(num_act <= THROTTLE_CURVE_RPM_ACT) + return; + + throttle_curve.rpm_meas = rpm[THROTTLE_CURVE_RPM_ACT]; + throttle_curve.rpm_measured = true; } /** * 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 motors_on, pprz_t in_cmd[]) +void throttle_curve_run(pprz_t cmds[], uint8_t ap_mode) { // 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; + if (ap_mode != AP_MODE_NAV) { + int8_t mode = ((float)(radio_control.values[RADIO_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL); + Bound(mode, 0, THROTTLE_CURVES_NB - 1); + throttle_curve.mode = mode; + } else { + throttle_curve.mode = throttle_curve.nav_mode; + } + + // Failsafe curve + if (ap_mode == AP_MODE_FAILSAFE) { + throttle_curve.mode = 0; + } // Check if we have multiple points or a single point - struct curve_t curve = throttle_curve.curves[mode]; + struct curve_t curve = throttle_curve.curves[throttle_curve.mode]; if (curve.nb_points == 1) { throttle_curve.throttle = curve.throttle[0]; throttle_curve.collective = curve.collective[0]; + throttle_curve.rpm = curve.rpm[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); + int8_t curve_p = ((float)cmds[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; + // Calculate the throttle, pitch and rpm value + uint16_t x = cmds[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); + if (curve.rpm[0] != 0xFFFF) { + if (throttle_curve.rpm == 0xFFFF) { + throttle_curve.rpm = throttle_curve.rpm_meas; + } + uint16_t new_rpm = curve.rpm[curve_p] + + ((curve.rpm[curve_p + 1] - curve.rpm[curve_p]) * x / curve_range); + int32_t rpm_diff = new_rpm - throttle_curve.rpm; + Bound(rpm_diff, -(THROTTLE_CURVE_RPM_INC_LIMIT/512), THROTTLE_CURVE_RPM_INC_LIMIT/512); + throttle_curve.rpm += rpm_diff; + } else { + throttle_curve.rpm = 0xFFFF; + } } + // Trim + int32_t trimmed_throttle = throttle_curve.throttle + throttle_curve.throttle_trim; + Bound(trimmed_throttle, 0, MAX_PPRZ); + throttle_curve.throttle = trimmed_throttle; + + int32_t trimmed_collective = throttle_curve.collective + throttle_curve.coll_trim; + Bound(trimmed_collective, -MAX_PPRZ, MAX_PPRZ); + throttle_curve.collective = trimmed_collective; + + // Update RPM feedback + if (curve.rpm[0] != 0xFFFF && throttle_curve.rpm_measured) { + // Calculate RPM error + int32_t rpm_err = (throttle_curve.rpm - throttle_curve.rpm_meas); + + // Calculate integrated error + throttle_curve.rpm_err_sum += rpm_err * throttle_curve.rpm_fb_i / 512.0f; + Bound(throttle_curve.rpm_err_sum, -throttle_curve.throttle, (MAX_PPRZ - throttle_curve.throttle)); + + // Calculate feedback command + int32_t rpm_feedback = rpm_err * throttle_curve.rpm_fb_p + throttle_curve.rpm_err_sum; + Bound(rpm_feedback, -MAX_PPRZ, MAX_PPRZ); + + // Apply feedback command + int32_t new_throttle = throttle_curve.throttle + rpm_feedback; + Bound(new_throttle, 0, MAX_PPRZ); + throttle_curve.throttle = new_throttle; + throttle_curve.rpm_measured = false; + } else if (curve.rpm[0] == 0xFFFF) { + throttle_curve.rpm_err_sum = 0; + } + + // Set the commands + cmds[COMMAND_THRUST] = throttle_curve.throttle; //Reuse for now + cmds[COMMAND_COLLECTIVE] = throttle_curve.collective; + // Only set throttle if motors are on - if (!motors_on) { - throttle_curve.throttle = 0; + if (!autopilot_get_motors_on()) { + cmds[COMMAND_THRUST] = 0; + throttle_curve.rpm_err_sum = 0; } } @@ -87,5 +207,7 @@ void throttle_curve_run(bool motors_on, pprz_t in_cmd[]) */ void nav_throttle_curve_set(uint8_t mode) { - commands[COMMAND_FMODE] = mode * THROTTLE_CURVE_SWITCH_VAL - MAX_PPRZ; + int16_t new_mode = mode; + Bound(new_mode, 0, THROTTLE_CURVES_NB - 1); + throttle_curve.nav_mode = new_mode; } diff --git a/sw/airborne/modules/helicopter/throttle_curve.h b/sw/airborne/modules/helicopter/throttle_curve.h index 4c70b950a8..eb2f877c6b 100644 --- a/sw/airborne/modules/helicopter/throttle_curve.h +++ b/sw/airborne/modules/helicopter/throttle_curve.h @@ -35,23 +35,35 @@ 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 + uint16_t rpm[THROTTLE_POINTS_NB]; ///< RPM 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 nav_mode; ///< Nav 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 + uint16_t rpm; ///< Output RPM of the throttle curve + + uint16_t rpm_meas; ///< RPM measured + bool rpm_measured; ///< Whenever the RPM is measured + float rpm_err_sum; ///< Summed RPM error + float rpm_fb_p; ///< RPM feedback p gain + float rpm_fb_i; + + int32_t throttle_trim; ///< RPM feedback i gain + int32_t coll_trim; ///< Collective trim }; extern struct throttle_curve_t throttle_curve; /* External functions */ extern void throttle_curve_init(void); -void throttle_curve_run(bool motors_on, pprz_t in_cmd[]); +void throttle_curve_run(pprz_t in_cmd[], uint8_t autopilot_mode); void nav_throttle_curve_set(uint8_t mode); #endif diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index 1a344af611..f84ecf1d22 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -289,14 +289,28 @@ 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 +let parse_heli_curves = fun heli_curves -> + let a = fun s -> ExtXml.attrib heli_curves s in + match Xml.tag heli_curves with "curve" -> let throttle = a "throttle" in + let rpm = ExtXml.attrib_or_default heli_curves "rpm" "" in let collective = a "collective" in - printf " {.nb_points = %i, \\\n" (List.length (Str.split (Str.regexp ",") throttle)); + let nb_throttle = List.length (Str.split (Str.regexp ",") throttle) in + let nb_rpm = List.length (Str.split (Str.regexp ",") rpm) in + let nb_collective = List.length (Str.split (Str.regexp ",") collective) in + if nb_throttle < 1 then + failwith (Printf.sprintf "Need at least one value in throttle curve for a throttle ('%s', '%s')" throttle collective); + if nb_throttle <> nb_collective then + failwith (Printf.sprintf "Amount of throttle points not the same as collective in throttle curve ('%s', '%s')" throttle collective); + if nb_throttle <> nb_rpm && nb_rpm <> 0 then + failwith (Printf.sprintf "Amount of throttle points not the same as rpm in throttle curve ('%s', '%s', '%s')" throttle collective rpm); + printf " {.nb_points = %i, \\\n" nb_throttle; printf " .throttle = {%s}, \\\n" throttle; + if nb_rpm <> 0 then + printf " .rpm = {%s}, \\\n" rpm + else + printf " .rpm = {0xFFFF}, \\\n"; printf " .collective = {%s}}, \\\n" collective | _ -> xml_error "mixer" From 285a72c93137c97d351a659b6670adaaac860016 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Thu, 21 Jun 2018 15:36:03 +0200 Subject: [PATCH 2/2] Update airframes with throttle curves --- conf/airframes/tudelft/bs_helidd_indi.xml | 1 - conf/airframes/tudelft/bs_helidd_pid.xml | 1 - conf/airframes/tudelft/heli450.xml | 1 - conf/airframes/tudelft/heliGeniusDD.xml | 1 - conf/airframes/tudelft/logo600.xml | 8 ++++---- conf/airframes/tudelft/walkera_genius_v2.xml | 1 - conf/conf_tests.xml | 16 ++++++++-------- conf/userconf/tudelft/conf.xml | 2 +- 8 files changed, 13 insertions(+), 18 deletions(-) diff --git a/conf/airframes/tudelft/bs_helidd_indi.xml b/conf/airframes/tudelft/bs_helidd_indi.xml index a0dd451be5..b198316246 100644 --- a/conf/airframes/tudelft/bs_helidd_indi.xml +++ b/conf/airframes/tudelft/bs_helidd_indi.xml @@ -79,7 +79,6 @@
- diff --git a/conf/airframes/tudelft/bs_helidd_pid.xml b/conf/airframes/tudelft/bs_helidd_pid.xml index eb05ec3f17..dd543b33e2 100644 --- a/conf/airframes/tudelft/bs_helidd_pid.xml +++ b/conf/airframes/tudelft/bs_helidd_pid.xml @@ -78,7 +78,6 @@ - diff --git a/conf/airframes/tudelft/heli450.xml b/conf/airframes/tudelft/heli450.xml index 97be2b2c87..79b91852e8 100644 --- a/conf/airframes/tudelft/heli450.xml +++ b/conf/airframes/tudelft/heli450.xml @@ -63,7 +63,6 @@ - diff --git a/conf/airframes/tudelft/heliGeniusDD.xml b/conf/airframes/tudelft/heliGeniusDD.xml index 9871097711..a8aec0231a 100644 --- a/conf/airframes/tudelft/heliGeniusDD.xml +++ b/conf/airframes/tudelft/heliGeniusDD.xml @@ -78,7 +78,6 @@ - diff --git a/conf/airframes/tudelft/logo600.xml b/conf/airframes/tudelft/logo600.xml index 1225b69caf..e0c107d8a8 100644 --- a/conf/airframes/tudelft/logo600.xml +++ b/conf/airframes/tudelft/logo600.xml @@ -41,6 +41,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea + @@ -54,7 +55,6 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea - @@ -88,6 +88,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea + @@ -111,14 +112,13 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea - - + - + diff --git a/conf/airframes/tudelft/walkera_genius_v2.xml b/conf/airframes/tudelft/walkera_genius_v2.xml index c28d7c220e..6ac0bcfc67 100644 --- a/conf/airframes/tudelft/walkera_genius_v2.xml +++ b/conf/airframes/tudelft/walkera_genius_v2.xml @@ -165,7 +165,6 @@ Using Lisa/S V1.0 board file as it is software compatible. - diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index a6c96d2953..d3ba01cc6b 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -35,12 +35,12 @@