diff --git a/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml b/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml new file mode 100644 index 0000000000..a9910c8c1d --- /dev/null +++ b/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml @@ -0,0 +1,98 @@ + + + +
+#include "autopilot.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 61a9ab4d35..993e43eaec 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -27,17 +27,18 @@ - + - - + + + @@ -125,7 +126,7 @@ value CDATA #REQUIRED> - - - + + + + + + Helicopter Rotor spinup + This module controls the spinup of the main rotor from a Helicopter. + - First make sure you are in the correct throttle curve + - Next call nav_heli_spinup_setup with a duration and end throttle level + - Then call nav_heli_spinup_run + + +
+ +
+ + + + + diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index f80821c2a6..bd7efb9944 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -210,6 +210,9 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr {h_ctl_roll_setpoint = _roll;} \ } +#define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"") + + #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; } #define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 349007a63a..33568e1de4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -404,12 +404,18 @@ void guidance_h_run(bool in_flight) guidance_h_nav_enter(); } - if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { + if(horizontal_mode == HORIZONTAL_MODE_MANUAL) { + stabilization_cmd[COMMAND_ROLL] = nav_roll; + stabilization_cmd[COMMAND_PITCH] = nav_pitch; + stabilization_cmd[COMMAND_YAW] = nav_heading; + } + else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { struct Int32Eulers sp_cmd_i; sp_cmd_i.phi = nav_roll; sp_cmd_i.theta = nav_pitch; sp_cmd_i.psi = nav_heading; stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); + stabilization_attitude_run(in_flight); } else { INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot); @@ -427,8 +433,8 @@ void guidance_h_run(bool in_flight) stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading); #endif + stabilization_attitude_run(in_flight); } - stabilization_attitude_run(in_flight); break; #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index d3476c649e..a5bc184797 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -479,6 +479,20 @@ void nav_home(void) nav_run(); } +/** Set manual roll, pitch and yaw without stabilization + * + * @param[in] roll The angle in radians (float) + * @param[in] pitch The angle in radians (float) + * @param[in] yaw The angle in radians (float) + */ +void nav_set_manual(float roll, float pitch, float yaw) +{ + horizontal_mode = HORIZONTAL_MODE_MANUAL; + nav_roll = ANGLE_BFP_OF_REAL(roll); + nav_pitch = ANGLE_BFP_OF_REAL(pitch); + nav_heading = ANGLE_BFP_OF_REAL(yaw); +} + /** Returns squared horizontal distance to given point */ float get_dist2_to_point(struct EnuCoor_i *p) { diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index cacf10767b..2400c12340 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -52,6 +52,7 @@ extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians; #define HORIZONTAL_MODE_ROUTE 1 #define HORIZONTAL_MODE_CIRCLE 2 #define HORIZONTAL_MODE_ATTITUDE 3 +#define HORIZONTAL_MODE_MANUAL 4 extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC extern float nav_radius; @@ -80,6 +81,7 @@ extern float get_dist2_to_waypoint(uint8_t wp_id); extern float get_dist2_to_point(struct EnuCoor_i *p); extern void compute_dist2_to_home(void); extern void nav_home(void); +extern void nav_set_manual(float roll, float pitch, float yaw); unit_t nav_reset_reference(void) __attribute__((unused)); unit_t nav_reset_alt(void) __attribute__((unused)); @@ -208,6 +210,8 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time); nav_roll = ANGLE_BFP_OF_REAL(_roll); \ } +#define NavSetManual nav_set_manual + #define NavStartDetectGround() ({ autopilot_detect_ground_once = true; false; }) #define NavDetectGround() nav_detect_ground() diff --git a/sw/airborne/modules/helicopter/throttle_curve.c b/sw/airborne/modules/helicopter/throttle_curve.c index 6304a281a1..3f7d24ac3c 100644 --- a/sw/airborne/modules/helicopter/throttle_curve.c +++ b/sw/airborne/modules/helicopter/throttle_curve.c @@ -25,6 +25,7 @@ */ #include "throttle_curve.h" +#include "subsystems/commands.h" /* The switching values for the Throttle Curve Mode switch */ #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB) @@ -80,3 +81,11 @@ void throttle_curve_run(bool motors_on, pprz_t in_cmd[]) throttle_curve.throttle = 0; } } + +/** + * Set a specific throttle curve based on the mode given with this function + */ +void nav_throttle_curve_set(uint8_t mode) +{ + commands[COMMAND_FMODE] = mode * THROTTLE_CURVE_SWITCH_VAL - MAX_PPRZ; +} diff --git a/sw/airborne/modules/helicopter/throttle_curve.h b/sw/airborne/modules/helicopter/throttle_curve.h index 9c7324f29b..4c70b950a8 100644 --- a/sw/airborne/modules/helicopter/throttle_curve.h +++ b/sw/airborne/modules/helicopter/throttle_curve.h @@ -52,5 +52,6 @@ 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 nav_throttle_curve_set(uint8_t mode); #endif diff --git a/sw/airborne/modules/nav/nav_heli_spinup.c b/sw/airborne/modules/nav/nav_heli_spinup.c new file mode 100644 index 0000000000..353d76d267 --- /dev/null +++ b/sw/airborne/modules/nav/nav_heli_spinup.c @@ -0,0 +1,58 @@ +/* + * Copyright (C) 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/nav/nav_heli_spinup.c" + * @author Freek van Tienen + * This module controls the spinup of the main rotor from a Helicopter + */ + +#include "modules/nav/nav_heli_spinup.h" +#include "navigation.h" +#include "paparazzi.h" + +static struct nav_heli_spinup_t nav_heli_spinup; + +void nav_heli_spinup_setup(uint16_t duration, float throttle) +{ + nav_heli_spinup.duration = (duration > 0) ? duration : 1; + nav_heli_spinup.throttle = throttle * MAX_PPRZ; + + nav_throttle = 0; + nav_roll = 0; + nav_pitch = 0; + nav_heading = 0; + horizontal_mode = HORIZONTAL_MODE_MANUAL; + vertical_mode = VERTICAL_MODE_MANUAL; +} + +bool nav_heli_spinup_run(void) +{ + if (stage_time > nav_heli_spinup.duration) { + return false; + } + + nav_roll = 0; + nav_pitch = 0; + nav_heading = 0; + horizontal_mode = HORIZONTAL_MODE_MANUAL; + vertical_mode = VERTICAL_MODE_MANUAL; + nav_throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration; + return true; +} diff --git a/sw/airborne/modules/nav/nav_heli_spinup.h b/sw/airborne/modules/nav/nav_heli_spinup.h new file mode 100644 index 0000000000..bac44a0e6c --- /dev/null +++ b/sw/airborne/modules/nav/nav_heli_spinup.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 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/nav/nav_heli_spinup.h" + * @author Freek van Tienen + * This module controls the spinup of the main rotor from a Helicopter + */ + +#ifndef NAV_HELI_SPINUP_H +#define NAV_HELI_SPINUP_H + +#include "std.h" + +struct nav_heli_spinup_t { + uint16_t duration; ///< The duration in seconds to reach the final throttle + uint32_t throttle; ///< The final throttle level +}; + +/** Initialization function + * + * Called in the flight plan before the 'run' function + * + * @param[in] duration The duration in seconds before reaching final throttle level + * @param[in] throttle The throttle value to go to + */ +extern void nav_heli_spinup_setup(uint16_t duration, float throttle); + +/** Heli spinup run function + * + * Controls the spinup of the throttle without stabilization + * + * @return true until the takeoff procedure ends + */ +extern bool nav_heli_spinup_run(void); + +#endif diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index 959859ced6..636a35da33 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -14,7 +14,7 @@ let gps_modes = [|"NOFIX";"NA";"2D";"3D";"DGPS";"RTK"|] let state_filter_modes = [|"UNKNOWN";"INIT";"ALIGN";"OK";"GPS_LOST";"IMU_LOST";"COV_ERR";"IR_CONTRAST";"ERROR"|] let _3D = 3 let gps_hybrid_modes = [|"OFF";"ON"|] -let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|] +let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE";"MANUAL"|] let if_modes = [|"OFF";"DOWN";"UP"|] let string_of_values = fun values -> diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index 486f4e2a5e..d1bdd83799 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -213,12 +213,16 @@ let pprz_throttle = fun s -> (********************* Vertical control ********************************************) let output_vmode = fun stage_xml wp last_wp -> let pitch = try Xml.attrib stage_xml "pitch" with _ -> "0.0" in - if pitch = "auto" + if String.lowercase (Xml.tag stage_xml) <> "manual" then begin - lprintf "NavVerticalAutoPitchMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle")) - end else begin - lprintf "NavVerticalAutoThrottleMode(RadOfDeg(%s));\n" (parse pitch); + if pitch = "auto" + then begin + lprintf "NavVerticalAutoPitchMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle")) + end else begin + lprintf "NavVerticalAutoThrottleMode(RadOfDeg(%s));\n" (parse pitch); + end end; + let vmode = try ExtXml.attrib stage_xml "vmode" with _ -> "alt" in begin match vmode with @@ -300,7 +304,7 @@ let rec index_stage = fun x -> incr stage; (* To count the loop stage *) Xml.Element (Xml.tag x, Xml.attribs x@["no", soi n], l) | "return" | "goto" | "deroute" | "exit_block" | "follow" | "call" | "home" - | "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" -> + | "heading" | "attitude" | "manual" | "go" | "stay" | "xyz" | "set" | "circle" -> incr stage; Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x) | "survey_rectangle" | "eight" | "oval"-> @@ -396,6 +400,20 @@ let rec print_stage = fun index_of_waypoints x -> ignore (output_vmode x "" ""); left (); lprintf "}\n"; lprintf "break;\n" + | "manual" -> + stage (); + begin + try + let until = parsed_attrib x "until" in + lprintf "if (%s) NextStageAndBreak() else {\n" until; + with ExtXml.Error _ -> + lprintf "{\n" + end; + right (); + lprintf "NavSetManual(%s, %s, %s);\n" (parsed_attrib x "roll") (parsed_attrib x "pitch") (parsed_attrib x "yaw"); + ignore (output_vmode x "" ""); + left (); lprintf "}\n"; + lprintf "break;\n" | "go" -> stage (); let wp =