diff --git a/conf/airframes/TUDelft/airframes/bebop_flip.xml b/conf/airframes/TUDelft/airframes/bebop_flip.xml new file mode 100644 index 00000000000..adef3a33dd6 --- /dev/null +++ b/conf/airframes/TUDelft/airframes/bebop_flip.xml @@ -0,0 +1,248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + +
+ + + + +
+ + + + + + + + + +
+ + + +
+ +
+ + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ + + + + +
+ diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index bba591c1c79..4d3b788cf52 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -103,6 +103,7 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c $(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c $(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c $(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c +$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_flip.c include $(CFG_ROTORCRAFT)/navigation.makefile diff --git a/conf/messages.xml b/conf/messages.xml index 801f809e7d7..0e884d14aac 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1988,7 +1988,7 @@ - + diff --git a/conf/settings/rotorcraft_basic.xml b/conf/settings/rotorcraft_basic.xml index dfffb73ad6c..73b86304fbd 100644 --- a/conf/settings/rotorcraft_basic.xml +++ b/conf/settings/rotorcraft_basic.xml @@ -4,7 +4,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 4ce219c9b46..16af397425c 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -436,6 +436,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING); #endif break; + case AP_MODE_FLIP: + guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP); + break; default: break; } @@ -482,6 +485,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING); #endif break; + case AP_MODE_FLIP: + guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP); + break; default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index c1a1cb35582..b805097e46c 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -51,6 +51,7 @@ #define AP_MODE_CARE_FREE_DIRECT 15 #define AP_MODE_FORWARD 16 #define AP_MODE_MODULE 17 +#define AP_MODE_FLIP 18 extern uint8_t autopilot_mode; extern uint8_t autopilot_mode_auto2; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c new file mode 100644 index 00000000000..b1ba0cf9efa --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * 2015 Ewoud Smeur + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_flip.c + * + * Open Loop guidance for making a flip. You need to tune this before using. + * When entering this mode it saves the previous guidance mode and changes AUTO2 back to + * the previous mode after finishing the flip. + * Use it with caution! + */ + +#include "guidance_flip.h" + +#include "autopilot.h" +#include "guidance_h.h" +#include "stabilization/stabilization_attitude_rc_setpoint.h" +#include "stabilization/stabilization_attitude.h" + +#define STOP_ROLL_CMD_ANGLE 25.0 +#define FIRST_THRUST_DURATION 0.3 +#define FINAL_THRUST_LEVEL 6000 + +uint32_t flip_counter; +uint8_t flip_state; +bool_t flip_rollout; +int32_t heading_save; +uint8_t autopilot_mode_old; +struct Int32Vect2 flip_cmd_earth; + +void guidance_flip_enter(void) +{ + flip_counter = 0; + flip_state = 0; + flip_rollout = false; + heading_save = stabilization_attitude_get_heading_i(); + autopilot_mode_old = autopilot_mode; +} + +void guidance_flip_run(void) +{ + uint32_t timer; + int32_t phi; + static uint32_t timer_save = 0; + + timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY; + phi = stateGetNedToBodyEulers_i()->phi; + + switch (flip_state) { + case 0: + flip_cmd_earth.x = 0; + flip_cmd_earth.y = 0; + stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth, + heading_save); + stabilization_attitude_run(autopilot_in_flight); + stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first + timer_save = 0; + + if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) { + flip_state++; + } + break; + + case 1: + stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command + stabilization_cmd[COMMAND_PITCH] = 0; + stabilization_cmd[COMMAND_YAW] = 0; + stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust? + + if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) { + flip_state++; + } + break; + + case 2: + stabilization_cmd[COMMAND_ROLL] = 0; + stabilization_cmd[COMMAND_PITCH] = 0; + stabilization_cmd[COMMAND_YAW] = 0; + stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust? + + if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) { + timer_save = timer; + flip_state++; + } + break; + + case 3: + flip_cmd_earth.x = 0; + flip_cmd_earth.y = 0; + stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth, + heading_save); + stabilization_attitude_run(autopilot_in_flight); + + stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling + + if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) { + flip_state++; + } + break; + + default: + autopilot_mode_auto2 = autopilot_mode_old; + autopilot_set_mode(autopilot_mode_old); + stab_att_sp_euler.psi = heading_save; + flip_rollout = false; + flip_counter = 0; + timer_save = 0; + flip_state = 0; + + stabilization_cmd[COMMAND_ROLL] = 0; + stabilization_cmd[COMMAND_PITCH] = 0; + stabilization_cmd[COMMAND_YAW] = 0; + stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll? + break; + } +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.h new file mode 100644 index 00000000000..f41b3de3250 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * 2015 Ewoud Smeur + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_flip.h + * + * Open Loop guidance for making a flip. You need to tune this before using. + * When entering this mode it saves the previous guidance mode and changes AUTO2 back to + * the previous mode after finishing the flip. + * Use it with caution! + */ + +#ifndef GUIDANCE_FLIP_H +#define GUIDANCE_FLIP_H + +void guidance_flip_enter(void); +void guidance_flip_run(void); + +#endif /* GUIDANCE_FLIP_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 765698294f9..cc4e92a9ea8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -27,6 +27,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "firmwares/rotorcraft/guidance/guidance_flip.h" #include "firmwares/rotorcraft/guidance/guidance_module.h" #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" @@ -263,6 +264,10 @@ void guidance_h_mode_changed(uint8_t new_mode) stabilization_attitude_enter(); break; + case GUIDANCE_H_MODE_FLIP: + guidance_flip_enter(); + break; + default: break; } @@ -317,13 +322,15 @@ void guidance_h_read_rc(bool_t in_flight) INT_EULERS_ZERO(guidance_h.rc_sp); } break; + case GUIDANCE_H_MODE_FLIP: + stabilization_attitude_read_rc(in_flight, FALSE, FALSE); + break; default: break; } } - void guidance_h_run(bool_t in_flight) { switch (guidance_h.mode) { @@ -399,6 +406,10 @@ void guidance_h_run(bool_t in_flight) break; #endif + case GUIDANCE_H_MODE_FLIP: + guidance_flip_run(); + break; + default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index c9b849716b0..844c7a8905d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -58,6 +58,7 @@ #define GUIDANCE_H_MODE_CARE_FREE 6 #define GUIDANCE_H_MODE_FORWARD 7 #define GUIDANCE_H_MODE_MODULE 8 +#define GUIDANCE_H_MODE_FLIP 9 struct HorizontalGuidanceSetpoint { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index aa756bccc84..cc439a82a4a 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -248,6 +248,9 @@ void guidance_v_mode_changed(uint8_t new_mode) break; #endif + case GUIDANCE_V_MODE_FLIP: + break; + default: break; @@ -351,6 +354,10 @@ void guidance_v_run(bool_t in_flight) stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; } + + case GUIDANCE_V_MODE_FLIP: + break; + default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 49053be7fd7..3478efca17f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -39,6 +39,7 @@ #define GUIDANCE_V_MODE_HOVER 4 #define GUIDANCE_V_MODE_NAV 5 #define GUIDANCE_V_MODE_MODULE 6 +#define GUIDANCE_V_MODE_FLIP 7 extern uint8_t guidance_v_mode; diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 3d330332197..3fb621d2670 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1292,7 +1292,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> match ap_mode with "AUTO2" | "NAV" -> ok_color | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" | "MODULE" -> "#10F0E0" - | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color + | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" | "FLIP" -> warning_color | _ -> alert_color in ac.strip#set_color "AP" color; end;