diff --git a/conf/airframes/TUDelft/airframes/heli450.xml b/conf/airframes/TUDelft/airframes/heli450.xml new file mode 100644 index 0000000000..8e0b94c6b2 --- /dev/null +++ b/conf/airframes/TUDelft/airframes/heli450.xml @@ -0,0 +1,221 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + + + + + + + + + + +
+ + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml index 3a6bdeda39..ad2eb5c13d 100644 --- a/conf/airframes/TUDelft/conf.xml +++ b/conf/airframes/TUDelft/conf.xml @@ -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" /> + - + @@ -8,6 +8,7 @@ + @@ -15,6 +16,7 @@ + @@ -57,6 +59,8 @@ driver CDATA #IMPLIED> + + @@ -68,6 +72,10 @@ min CDATA #REQUIRED neutral CDATA #REQUIRED max CDATA #REQUIRED> + + diff --git a/conf/modules/heli_swashplate_mixing.xml b/conf/modules/heli_swashplate_mixing.xml new file mode 100644 index 0000000000..8740354b9d --- /dev/null +++ b/conf/modules/heli_swashplate_mixing.xml @@ -0,0 +1,17 @@ + + + + + Helicopter Swashplate Mixing + + heli_throttle_curve.xml +
+ +
+ + + + + + + diff --git a/conf/modules/heli_throttle_curve.xml b/conf/modules/heli_throttle_curve.xml new file mode 100644 index 0000000000..8b036cb8ca --- /dev/null +++ b/conf/modules/heli_throttle_curve.xml @@ -0,0 +1,15 @@ + + + + + Throttle Curve Mixers + +
+ +
+ + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index e3d5456aee..8602ea5d12 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -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 diff --git a/sw/airborne/modules/helicopter/swashplate_mixing.c b/sw/airborne/modules/helicopter/swashplate_mixing.c new file mode 100644 index 0000000000..41e4864645 --- /dev/null +++ b/sw/airborne/modules/helicopter/swashplate_mixing.c @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2015 C. De Wagter + * 2015 Freek van Tienen + * + * 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 + * . + */ +/** + * @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); + } +} diff --git a/sw/airborne/modules/helicopter/swashplate_mixing.h b/sw/airborne/modules/helicopter/swashplate_mixing.h new file mode 100644 index 0000000000..57c78ece2d --- /dev/null +++ b/sw/airborne/modules/helicopter/swashplate_mixing.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2015 C. De Wagter + * 2015 Freek van Tienen + * + * 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 + * . + */ +/** + * @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 diff --git a/sw/airborne/modules/helicopter/throttle_curve.c b/sw/airborne/modules/helicopter/throttle_curve.c new file mode 100644 index 0000000000..b709d99e5f --- /dev/null +++ b/sw/airborne/modules/helicopter/throttle_curve.c @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2015 C. De Wagter + * 2015 Freek van Tienen + * + * 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 + * . + */ +/** + * @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; + } +} diff --git a/sw/airborne/modules/helicopter/throttle_curve.h b/sw/airborne/modules/helicopter/throttle_curve.h new file mode 100644 index 0000000000..8fb4fef2f2 --- /dev/null +++ b/sw/airborne/modules/helicopter/throttle_curve.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2015 C. De Wagter + * 2015 Freek van Tienen + * + * 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 + * . + */ +/** + * @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 diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index a2401ecab8..3a0a367fcf 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -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