diff --git a/conf/airframes/DB/db_conf.xml b/conf/airframes/DB/db_conf.xml
index ad77277399..d73154e2b5 100644
--- a/conf/airframes/DB/db_conf.xml
+++ b/conf/airframes/DB/db_conf.xml
@@ -7,7 +7,7 @@
telemetry="telemetry/DB/db_default_rotorcraft.xml"
flight_plan="flight_plans/DB/db_outdoors.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/rotorcraft_speed.xml settings/control/stabilization_indi.xml [settings/control/stabilization_rate.xml] [settings/control/stabilization_att_int.xml] [settings/control/stabilization_att_int_quat.xml] [settings/control/stabilization_att_float_euler.xml]"
- settings_modules="modules/geo_mag.xml modules/air_data.xml"
+ settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="#0d890debf1c6"
/>
+
diff --git a/conf/airframes/DB/db_mavshot.xml b/conf/airframes/DB/db_mavshot.xml
index 95402ac09f..3d691fded0 100644
--- a/conf/airframes/DB/db_mavshot.xml
+++ b/conf/airframes/DB/db_mavshot.xml
@@ -278,12 +278,13 @@
-
+
+
diff --git a/conf/airframes/DB/db_quadshot_asp21_spektrum.xml b/conf/airframes/DB/db_quadshot_asp21_spektrum.xml
new file mode 100644
index 0000000000..3f96b0d7c7
--- /dev/null
+++ b/conf/airframes/DB/db_quadshot_asp21_spektrum.xml
@@ -0,0 +1,384 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/quadshot_178_pylons.xml b/conf/airframes/examples/quadshot_178_pylons.xml
new file mode 100644
index 0000000000..13395230bd
--- /dev/null
+++ b/conf/airframes/examples/quadshot_178_pylons.xml
@@ -0,0 +1,367 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml
index 9703263b3a..3f96b0d7c7 100644
--- a/conf/airframes/examples/quadshot_asp21_spektrum.xml
+++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml
@@ -4,12 +4,18 @@
-
-
-
-
-
-
+
+
+
+
+
+
+
+
@@ -26,24 +32,30 @@
-
-
-
-
+
+
+
+
-
-
+
+
-
-
-
+
+
+
-
-
+
+
+
+
+
+
+
@@ -74,12 +90,19 @@
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -91,31 +114,47 @@
+
-
-
-
+
+
+
-
-
+
+
+
-
-
+
+
@@ -130,7 +169,7 @@
-
+
@@ -147,17 +186,17 @@
-
-
-
+
+
+
-
-
+
+
-
-
+
+
@@ -170,71 +209,155 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
-
+
+
+
-
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
-
-
-
+
+
+
+
-
-
-
+
+
+
+
+
+
+
+
+
-
-
+
+
+
+
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
+
@@ -244,13 +367,16 @@
+
-
+
+
+
-
-
+
+
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index c4251a3132..24011ddec7 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -183,7 +183,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_rate.xml] settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml settings/nps.xml"
- settings_modules="modules/gps_ubx_ucenter.xml"
+ settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml"
gui_color="#710080"
/>
+
+
+
+
+
+
+ #include "autopilot.h"
+ #include "subsystems/radio_control.h"
+ #include "subsystems/electrical.h"
+ #include "subsystems/actuators.h"
+ #include "firmwares/rotorcraft/guidance/guidance_h.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/quadshot_delft.xml b/conf/flight_plans/quadshot_delft.xml
new file mode 100644
index 0000000000..1d591b0880
--- /dev/null
+++ b/conf/flight_plans/quadshot_delft.xml
@@ -0,0 +1,111 @@
+
+
+
+
+
+
+
+ #include "autopilot.h"
+ #include "subsystems/radio_control.h"
+ #include "subsystems/electrical.h"
+ #include "subsystems/actuators.h"
+ #include "firmwares/rotorcraft/guidance/guidance_h.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/quadshot_land_takeoff_again.xml b/conf/flight_plans/quadshot_land_takeoff_again.xml
new file mode 100644
index 0000000000..892bb4df16
--- /dev/null
+++ b/conf/flight_plans/quadshot_land_takeoff_again.xml
@@ -0,0 +1,104 @@
+
+
+
+
+
+ #include "autopilot.h"
+ #include "subsystems/radio_control.h"
+ #include "subsystems/electrical.h"
+ #include "subsystems/actuators.h"
+ #include "firmwares/rotorcraft/guidance/guidance_h.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/gcs/DB/db_gcs_layout.xml b/conf/gcs/DB/db_gcs_layout.xml
index e0f665351d..896167a609 100644
--- a/conf/gcs/DB/db_gcs_layout.xml
+++ b/conf/gcs/DB/db_gcs_layout.xml
@@ -7,9 +7,8 @@
-
-
-
+
+
@@ -17,20 +16,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -46,6 +31,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/gcs/horizontal.xml b/conf/gcs/horizontal.xml
index 67582f3130..f956bba97a 100644
--- a/conf/gcs/horizontal.xml
+++ b/conf/gcs/horizontal.xml
@@ -1,14 +1,12 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/sonar_adc.xml b/conf/modules/sonar_adc.xml
index a0be44908c..321d1d716f 100644
--- a/conf/modules/sonar_adc.xml
+++ b/conf/modules/sonar_adc.xml
@@ -17,7 +17,7 @@
-
+
diff --git a/conf/settings/control/gain_scheduling.xml b/conf/settings/control/gain_scheduling.xml
new file mode 100644
index 0000000000..a131c39e1a
--- /dev/null
+++ b/conf/settings/control/gain_scheduling.xml
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/control/hybrid_guidance.xml b/conf/settings/control/hybrid_guidance.xml
new file mode 100644
index 0000000000..6528af4cd9
--- /dev/null
+++ b/conf/settings/control/hybrid_guidance.xml
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/DB/db_quadshot.xml b/conf/telemetry/DB/db_quadshot.xml
new file mode 100644
index 0000000000..4e2ed00c32
--- /dev/null
+++ b/conf/telemetry/DB/db_quadshot.xml
@@ -0,0 +1,167 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_quadshot.xml b/conf/telemetry/default_quadshot.xml
new file mode 100644
index 0000000000..7a05b91626
--- /dev/null
+++ b/conf/telemetry/default_quadshot.xml
@@ -0,0 +1,165 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index b358163be4..33676c1d63 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -112,6 +112,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 08b76d409d..04d5496370 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -26,6 +26,7 @@
#include "generated/airframe.h"
+#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_flip.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
@@ -201,6 +202,10 @@ void guidance_h_init(void)
#if GUIDANCE_INDI
guidance_indi_enter();
#endif
+
+#if HYBRID_NAVIGATION
+ guidance_hybrid_init();
+#endif
}
@@ -225,6 +230,10 @@ void guidance_h_mode_changed(uint8_t new_mode)
transition_theta_offset = 0;
}
+#if HYBRID_NAVIGATION
+ guidance_hybrid_norm_ref_airspeed = 0;
+#endif
+
switch (new_mode) {
case GUIDANCE_H_MODE_RC_DIRECT:
stabilization_none_enter();
@@ -416,7 +425,17 @@ void guidance_h_run(bool in_flight)
sp_cmd_i.psi = nav_heading;
stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
stabilization_attitude_run(in_flight);
+
+#if HYBRID_NAVIGATION
+ //make sure the heading is right before leaving horizontal_mode attitude
+ guidance_hybrid_reset_heading(&sp_cmd_i);
+#endif
} else {
+
+#if HYBRID_NAVIGATION
+ INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
+ guidance_hybrid_run();
+#else
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
guidance_h_update_reference();
@@ -424,6 +443,7 @@ void guidance_h_run(bool in_flight)
/* set psi command */
guidance_h.sp.heading = nav_heading;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
+
#if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading);
#else
@@ -432,6 +452,8 @@ void guidance_h_run(bool in_flight)
/* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
guidance_h.sp.heading);
+#endif
+
#endif
stabilization_attitude_run(in_flight);
}
@@ -705,3 +727,8 @@ bool guidance_h_set_guided_heading_rate(float rate)
}
return false;
}
+
+const struct Int32Vect2 *guidance_h_get_pos_err(void)
+{
+ return &guidance_h_pos_err;
+}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
index f3f4b56e1f..db9a30f73c 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
@@ -147,6 +147,12 @@ extern bool guidance_h_set_guided_vel(float vx, float vy);
*/
extern bool guidance_h_set_guided_heading_rate(float rate);
+/** Gets the position error
+ * @param none.
+ * @return Pointer to a structure containing x and y position errors
+ */
+extern const struct Int32Vect2 *guidance_h_get_pos_err(void);
+
/* Make sure that ref can only be temporarily disabled for testing,
* but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
*/
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
new file mode 100644
index 0000000000..2ef5289d34
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
@@ -0,0 +1,409 @@
+/*
+ * Copyright (C) 2014 Ewoud Smeur
+ * This is code for guidance of hybrid UAVs. It needs a simple velocity
+ * model to control the ground velocity of the UAV while estimating the
+ * wind velocity.
+ *
+ * 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_hybrid.c
+ * Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
+ *
+ * Functionality:
+ * 1) hover with (helicopter) thrust vectoring and align the heading with the wind vector.
+ * 2) Forward flight with using pitch and a bit of thrust to control altitude and
+ * heading to control the velocity vector
+ * 3) Transition between the two, with the possibility to fly at any airspeed
+ */
+
+#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
+#include "firmwares/rotorcraft/guidance/guidance_h.h"
+#include "subsystems/radio_control.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+
+/* for guidance_v_thrust_coeff */
+#include "firmwares/rotorcraft/guidance/guidance_v.h"
+
+// max airspeed for quadshot guidance
+#define MAX_AIRSPEED 15
+// high res frac for integration of angles
+#define INT32_ANGLE_HIGH_RES_FRAC 18
+
+// Variables used for settings
+int32_t guidance_hybrid_norm_ref_airspeed;
+float alt_pitch_gain = 0.3;
+int32_t max_airspeed = MAX_AIRSPEED;
+int32_t wind_gain;
+int32_t horizontal_speed_gain;
+float max_turn_bank;
+float turn_bank_gain;
+
+// Private variables
+static struct Int32Eulers guidance_hybrid_ypr_sp;
+static struct Int32Vect2 guidance_hybrid_airspeed_sp;
+static struct Int32Vect2 guidance_h_pos_err;
+static struct Int32Vect2 guidance_hybrid_airspeed_ref;
+static struct Int32Vect2 wind_estimate;
+static struct Int32Vect2 wind_estimate_high_res;
+static struct Int32Vect2 guidance_hybrid_ref_airspeed;
+
+static int32_t norm_sp_airspeed_disp;
+static int32_t heading_diff_disp;
+static int32_t omega_disp;
+static int32_t high_res_psi;
+static int32_t airspeed_sp_heading_disp;
+static bool guidance_hovering;
+static bool force_forward_flight;
+static int32_t v_control_pitch;
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+static void send_hybrid_guidance(struct transport_tx *trans, struct link_device *dev)
+{
+ struct NedCoor_i *pos = stateGetPositionNed_i();
+ struct NedCoor_i *speed = stateGetSpeedNed_i();
+ pprz_msg_send_HYBRID_GUIDANCE(trans, dev, AC_ID,
+ &(pos->x), &(pos->y),
+ &(speed->x), &(speed->y),
+ &wind_estimate.x, &wind_estimate.y,
+ &guidance_h_pos_err.x,
+ &guidance_h_pos_err.y,
+ &guidance_hybrid_airspeed_sp.x,
+ &guidance_hybrid_airspeed_sp.y,
+ &guidance_hybrid_norm_ref_airspeed,
+ &heading_diff_disp,
+ &guidance_hybrid_ypr_sp.phi,
+ &guidance_hybrid_ypr_sp.theta,
+ &guidance_hybrid_ypr_sp.psi);
+}
+
+#endif
+
+void guidance_hybrid_init(void)
+{
+
+ INT_EULERS_ZERO(guidance_hybrid_ypr_sp);
+ INT_VECT2_ZERO(guidance_hybrid_airspeed_sp);
+ INT_VECT2_ZERO(guidance_hybrid_airspeed_ref);
+
+ high_res_psi = 0;
+ guidance_hovering = true;
+ horizontal_speed_gain = 8;
+ guidance_hybrid_norm_ref_airspeed = 0;
+ max_turn_bank = 40.0;
+ turn_bank_gain = 0.8;
+ wind_gain = 64;
+ force_forward_flight = 0;
+ INT_VECT2_ZERO(wind_estimate);
+ INT_VECT2_ZERO(guidance_hybrid_ref_airspeed);
+ INT_VECT2_ZERO(wind_estimate_high_res);
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HYBRID_GUIDANCE, send_hybrid_guidance);
+#endif
+
+}
+
+#define INT32_ANGLE_HIGH_RES_NORMALIZE(_a) { \
+ while ((_a) > (INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) -= (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
+ while ((_a) < (-INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) += (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
+ }
+
+void guidance_hybrid_run(void)
+{
+ guidance_hybrid_determine_wind_estimate();
+ guidance_hybrid_position_to_airspeed();
+ guidance_hybrid_airspeed_to_attitude(&guidance_hybrid_ypr_sp);
+ guidance_hybrid_set_cmd_i(&guidance_hybrid_ypr_sp);
+}
+
+void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
+{
+ guidance_hybrid_ypr_sp.psi = sp_cmd->psi;
+ high_res_psi = sp_cmd->psi << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC);
+ stabilization_attitude_set_rpy_setpoint_i(sp_cmd);
+}
+
+/// Convert a required airspeed to a certain attitude for the Quadshot
+void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
+{
+
+ //notes:
+ //in forward flight, it is preferred to first get to min(airspeed_sp, airspeed_ref) and then change heading and then get to airspeed_sp
+ //in hover, just a gradual change is needed, or maybe not even needed
+ //changes between flight regimes should be handled
+
+ //determine the heading of the airspeed_sp vector
+ int32_t omega = 0;
+ float airspeed_sp_heading = atan2f((float) POS_FLOAT_OF_BFP(guidance_hybrid_airspeed_sp.y),
+ (float) POS_FLOAT_OF_BFP(guidance_hybrid_airspeed_sp.x));
+ //only for debugging
+ airspeed_sp_heading_disp = (int32_t)(DegOfRad(airspeed_sp_heading));
+
+ //The difference of the current heading with the required heading.
+ float heading_diff = airspeed_sp_heading - ANGLE_FLOAT_OF_BFP(ypr_sp->psi);
+ FLOAT_ANGLE_NORMALIZE(heading_diff);
+
+ //only for debugging
+ heading_diff_disp = (int32_t)(heading_diff / 3.14 * 180.0);
+
+ //calculate the norm of the airspeed setpoint
+ int32_t norm_sp_airspeed;
+ norm_sp_airspeed = int32_vect2_norm(&guidance_hybrid_airspeed_sp);
+
+ //reference goes with a steady pace towards the setpoint airspeed
+ //hold ref norm below 4 m/s until heading is aligned
+ if (!((norm_sp_airspeed > (4 << 8)) && (guidance_hybrid_norm_ref_airspeed < (4 << 8))
+ && (guidance_hybrid_norm_ref_airspeed > ((4 << 8) - 10)) && (fabs(heading_diff) > (5.0 / 180.0 * 3.14)))) {
+ guidance_hybrid_norm_ref_airspeed = guidance_hybrid_norm_ref_airspeed + ((int32_t)(norm_sp_airspeed >
+ guidance_hybrid_norm_ref_airspeed) * 2 - 1) * 3 / 2;
+ }
+
+ norm_sp_airspeed_disp = norm_sp_airspeed;
+
+ int32_t psi = ypr_sp->psi;
+ int32_t s_psi, c_psi;
+ PPRZ_ITRIG_SIN(s_psi, psi);
+ PPRZ_ITRIG_COS(c_psi, psi);
+
+ guidance_hybrid_ref_airspeed.x = (guidance_hybrid_norm_ref_airspeed * c_psi) >> INT32_TRIG_FRAC;
+ guidance_hybrid_ref_airspeed.y = (guidance_hybrid_norm_ref_airspeed * s_psi) >> INT32_TRIG_FRAC;
+
+ if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
+ /// if required speed is lower than 4 m/s act like a rotorcraft
+ // translate speed_sp into bank angle and heading
+
+ // change heading to direction of airspeed, faster if the airspeed is higher
+ if (heading_diff > 0.0) {
+ omega = (norm_sp_airspeed << (INT32_ANGLE_FRAC - INT32_POS_FRAC)) / 6;
+ } else if (heading_diff < 0.0) {
+ omega = (norm_sp_airspeed << (INT32_ANGLE_FRAC - INT32_POS_FRAC)) / -6;
+ }
+
+ if (omega > ANGLE_BFP_OF_REAL(0.8)) { omega = ANGLE_BFP_OF_REAL(0.8); }
+ if (omega < ANGLE_BFP_OF_REAL(-0.8)) { omega = ANGLE_BFP_OF_REAL(-0.8); }
+
+ // 2) calculate roll/pitch commands
+ struct Int32Vect2 hover_sp;
+ if (norm_sp_airspeed > (4 <<
+ 8)) { //if the setpoint is beyond 4m/s but the ref is not, the norm of the hover sp will stay at 4m/s
+ hover_sp.x = (guidance_hybrid_airspeed_sp.x << 8) / norm_sp_airspeed * 4;
+ hover_sp.y = (guidance_hybrid_airspeed_sp.y << 8) / norm_sp_airspeed * 4;
+ } else {
+ hover_sp.x = guidance_hybrid_airspeed_sp.x;
+ hover_sp.y = guidance_hybrid_airspeed_sp.y;
+ }
+
+ // gain of 10 means that for 4 m/s an angle of 40 degrees is needed
+ ypr_sp->theta = (((- (c_psi * hover_sp.x + s_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
+ ypr_sp->phi = ((((- s_psi * hover_sp.x + c_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
+ } else {
+ /// if required speed is higher than 4 m/s act like a fixedwing
+ // translate speed_sp into theta + thrust
+ // coordinated turns to change heading
+
+ // calculate required pitch angle from airspeed_sp magnitude
+ if (guidance_hybrid_norm_ref_airspeed > (15 << 8)) {
+ ypr_sp->theta = -ANGLE_BFP_OF_REAL(RadOfDeg(78.0));
+ } else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) {
+ ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (8 << 8)) * 2 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
+ RadOfDeg(68.0));
+ } else {
+ ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (4 << 8)) * 7 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
+ RadOfDeg(40.0));
+ }
+
+ // if the sp_airspeed is within hovering range, don't start a coordinated turn
+ if (norm_sp_airspeed < (4 << 8)) {
+ omega = 0;
+ ypr_sp->phi = 0;
+ } else { // coordinated turn
+ ypr_sp->phi = ANGLE_BFP_OF_REAL(heading_diff * turn_bank_gain);
+ if (ypr_sp->phi > ANGLE_BFP_OF_REAL(max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(max_turn_bank / 180.0 * M_PI); }
+ if (ypr_sp->phi < ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI); }
+
+ //feedforward estimate angular rotation omega = g*tan(phi)/v
+ omega = ANGLE_BFP_OF_REAL(9.81 / POS_FLOAT_OF_BFP(guidance_hybrid_norm_ref_airspeed) * tanf(ANGLE_FLOAT_OF_BFP(
+ ypr_sp->phi)));
+
+ if (omega > ANGLE_BFP_OF_REAL(0.7)) { omega = ANGLE_BFP_OF_REAL(0.7); }
+ if (omega < ANGLE_BFP_OF_REAL(-0.7)) { omega = ANGLE_BFP_OF_REAL(-0.7); }
+ }
+ }
+
+ //only for debugging purposes
+ omega_disp = omega;
+
+ //go to higher resolution because else the increment is too small to be added
+ high_res_psi += (omega << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)) / 512;
+
+ INT32_ANGLE_HIGH_RES_NORMALIZE(high_res_psi);
+
+ // go back to angle_frac
+ ypr_sp->psi = high_res_psi >> (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC);
+ ypr_sp->theta = ypr_sp->theta + v_control_pitch;
+}
+
+void guidance_hybrid_position_to_airspeed(void)
+{
+ /* compute position error */
+ VECT2_DIFF(guidance_h_pos_err, guidance_h.sp.pos, *stateGetPositionNed_i());
+
+ // Compute ground speed setpoint
+ struct Int32Vect2 guidance_hybrid_groundspeed_sp;
+ VECT2_SDIV(guidance_hybrid_groundspeed_sp, guidance_h_pos_err, horizontal_speed_gain);
+
+ int32_t norm_groundspeed_sp;
+ norm_groundspeed_sp = int32_vect2_norm(&guidance_hybrid_groundspeed_sp);
+
+ //create setpoint groundspeed with a norm of 15 m/s
+ if (force_forward_flight) {
+ //scale the groundspeed_sp to 15 m/s if large enough to begin with
+ if (norm_groundspeed_sp > 1 << 8) {
+ guidance_hybrid_groundspeed_sp.x = guidance_hybrid_groundspeed_sp.x * (max_airspeed << 8) / norm_groundspeed_sp;
+ guidance_hybrid_groundspeed_sp.y = guidance_hybrid_groundspeed_sp.y * (max_airspeed << 8) / norm_groundspeed_sp;
+ } else { //groundspeed_sp is very small, so continue with the current heading
+ int32_t psi = guidance_hybrid_ypr_sp.psi;
+ int32_t s_psi, c_psi;
+ PPRZ_ITRIG_SIN(s_psi, psi);
+ PPRZ_ITRIG_COS(c_psi, psi);
+ guidance_hybrid_groundspeed_sp.x = (15 * c_psi) >> (INT32_TRIG_FRAC - 8);
+ guidance_hybrid_groundspeed_sp.y = (15 * s_psi) >> (INT32_TRIG_FRAC - 8);
+ }
+ }
+
+ struct Int32Vect2 airspeed_sp;
+ INT_VECT2_ZERO(airspeed_sp);
+ VECT2_ADD(airspeed_sp, guidance_hybrid_groundspeed_sp);
+ VECT2_ADD(airspeed_sp, wind_estimate);
+
+ int32_t norm_airspeed_sp;
+ norm_airspeed_sp = int32_vect2_norm(&airspeed_sp);
+
+ //Check if the airspeed_sp is larger than the max airspeed. If so, give the wind cancellatioin priority.
+ if (norm_airspeed_sp > (max_airspeed << 8) && norm_groundspeed_sp > 0) {
+ int32_t av = INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.x, guidance_hybrid_groundspeed_sp.x,
+ 8) + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.y, guidance_hybrid_groundspeed_sp.y, 8);
+ int32_t bv = 2 * (INT_MULT_RSHIFT(wind_estimate.x, guidance_hybrid_groundspeed_sp.x,
+ 8) + INT_MULT_RSHIFT(wind_estimate.y, guidance_hybrid_groundspeed_sp.y, 8));
+ int32_t cv = INT_MULT_RSHIFT(wind_estimate.x, wind_estimate.x, 8) + INT_MULT_RSHIFT(wind_estimate.y, wind_estimate.y,
+ 8) - (max_airspeed << 8) * max_airspeed;
+
+ float dv = POS_FLOAT_OF_BFP(bv) * POS_FLOAT_OF_BFP(bv) - 4.0 * POS_FLOAT_OF_BFP(av) * POS_FLOAT_OF_BFP(cv);
+ float d_sqrt_f = sqrtf(dv);
+ int32_t d_sqrt = POS_BFP_OF_REAL(d_sqrt_f);
+
+ int32_t result = ((-bv + d_sqrt) << 8) / (2 * av);
+
+ guidance_hybrid_airspeed_sp.x = wind_estimate.x + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.x, result, 8);
+ guidance_hybrid_airspeed_sp.y = wind_estimate.y + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.y, result, 8);
+ } else {
+ // Add the wind to get the airspeed setpoint
+ guidance_hybrid_airspeed_sp = guidance_hybrid_groundspeed_sp;
+ VECT2_ADD(guidance_hybrid_airspeed_sp, wind_estimate);
+ }
+
+// limit the airspeed setpoint to 15 m/s, because else saturation+windup will occur
+ norm_airspeed_sp = int32_vect2_norm(&guidance_hybrid_airspeed_sp);
+ if (norm_airspeed_sp > (max_airspeed << 8)) {
+ guidance_hybrid_airspeed_sp.x = guidance_hybrid_airspeed_sp.x * (max_airspeed << 8) / norm_airspeed_sp;
+ guidance_hybrid_airspeed_sp.y = guidance_hybrid_airspeed_sp.y * (max_airspeed << 8) / norm_airspeed_sp;
+ }
+}
+
+void guidance_hybrid_determine_wind_estimate(void)
+{
+
+ /* compute speed error */
+ struct Int32Vect2 wind_estimate_measured;
+ struct Int32Vect2 measured_ground_speed;
+ INT32_VECT2_RSHIFT(measured_ground_speed, *stateGetSpeedNed_i(), 11);
+ VECT2_DIFF(wind_estimate_measured, guidance_hybrid_ref_airspeed, measured_ground_speed);
+
+ //Low pass wind_estimate, because we know the wind usually only changes slowly
+ //But not too slow, because the wind_estimate is also an adaptive element for the airspeed model inaccuracies
+ wind_estimate_high_res.x += (((wind_estimate_measured.x - wind_estimate.x) > 0) * 2 - 1) * wind_gain;
+ wind_estimate_high_res.y += (((wind_estimate_measured.y - wind_estimate.y) > 0) * 2 - 1) * wind_gain;
+
+ wind_estimate.x = ((wind_estimate_high_res.x) >> 8);
+ wind_estimate.y = ((wind_estimate_high_res.y) >> 8);
+}
+
+void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
+{
+ /// @todo calc sp_quat in fixed-point
+
+ /* orientation vector describing simultaneous rotation of roll/pitch */
+ struct FloatVect3 ov;
+ ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi);
+ ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta);
+ ov.z = 0.0;
+ /* quaternion from that orientation vector */
+ struct FloatQuat q_rp;
+ float_quat_of_orientation_vect(&q_rp, &ov);
+ struct Int32Quat q_rp_i;
+ QUAT_BFP_OF_REAL(q_rp_i, q_rp);
+
+ // get the vertical vector to rotate the roll pitch setpoint around
+ struct Int32Vect3 zaxis = {0, 0, 1};
+
+ /* get current heading setpoint */
+ struct Int32Quat q_yaw_sp;
+ int32_quat_of_axis_angle(&q_yaw_sp, &zaxis, sp_cmd->psi);
+
+ // first apply the roll/pitch setpoint and then the yaw
+ int32_quat_comp(&stab_att_sp_quat, &q_yaw_sp, &q_rp_i);
+
+ int32_eulers_of_quat(&stab_att_sp_euler, &stab_att_sp_quat);
+}
+
+void guidance_hybrid_vertical(void)
+{
+ if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
+ //if airspeed ref < 4 only thrust
+ stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+ v_control_pitch = 0;
+ guidance_v_kp = GUIDANCE_V_HOVER_KP;
+ guidance_v_kd = GUIDANCE_V_HOVER_KD;
+ guidance_v_ki = GUIDANCE_V_HOVER_KI;
+ } else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) { //if airspeed ref > 8 only pitch,
+ //at 15 m/s the thrust has to be 33%
+ stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ / 5 + (((guidance_hybrid_norm_ref_airspeed - (8 << 8)) / 7 *
+ (MAX_PPRZ / 3 - MAX_PPRZ / 5)) >> 8) + (guidance_v_delta_t - MAX_PPRZ / 2) / 10;
+ //stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ/5;
+ // stabilization_cmd[COMMAND_THRUST] = ((guidance_hybrid_norm_ref_airspeed - (8<<8)) / 7 * (MAX_PPRZ/3 - MAX_PPRZ/5))>>8 + 9600/5;
+ //Control altitude with pitch, now only proportional control
+ float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
+ v_control_pitch = ANGLE_BFP_OF_REAL(alt_control_pitch / (MAX_PPRZ * guidance_v_nominal_throttle));
+ guidance_v_kp = GUIDANCE_V_HOVER_KP / 2;
+ guidance_v_kd = GUIDANCE_V_HOVER_KD / 2;
+ guidance_v_ki = GUIDANCE_V_HOVER_KI / 2;
+ } else { //if airspeed ref > 4 && < 8 both
+ int32_t airspeed_transition = (guidance_hybrid_norm_ref_airspeed - (4 << 8)) / 4; //divide by 4 to scale it to 0-1 (<<8)
+ stabilization_cmd[COMMAND_THRUST] = ((MAX_PPRZ / 5 + (guidance_v_delta_t - MAX_PPRZ / 2) / 10) * airspeed_transition +
+ guidance_v_delta_t * ((1 << 8) - airspeed_transition)) >> 8;
+ float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
+ v_control_pitch = INT_MULT_RSHIFT((int32_t) ANGLE_BFP_OF_REAL(alt_control_pitch / (MAX_PPRZ *
+ guidance_v_nominal_throttle)), airspeed_transition, 8);
+ guidance_v_kp = GUIDANCE_V_HOVER_KP;
+ guidance_v_kd = GUIDANCE_V_HOVER_KD;
+ guidance_v_ki = GUIDANCE_V_HOVER_KI;
+ }
+}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
new file mode 100644
index 0000000000..69dd5cb279
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2014 Ewoud Smeur
+ * This is code for guidance of hybrid UAVs. It needs a simple velocity
+ * model to control the ground velocity of the UAV while estimating the
+ * wind velocity.
+ *
+ * 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_hybrid.h
+ * Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
+ *
+ */
+
+#ifndef GUIDANCE_HYBRID_H
+#define GUIDANCE_HYBRID_H
+
+#include "math/pprz_algebra_int.h"
+
+extern int32_t guidance_hybrid_norm_ref_airspeed;
+extern float alt_pitch_gain;
+extern int32_t max_airspeed;
+extern int32_t wind_gain;
+extern int32_t horizontal_speed_gain;
+extern float max_turn_bank;
+extern float turn_bank_gain;
+
+/** Runs the Hybrid Guidance main functions.
+ */
+extern void guidance_hybrid_run(void);
+
+/** Hybrid Guidance Initialization function.
+ * @param
+ */
+extern void guidance_hybrid_init(void);
+
+/** Creates the attitude set-points from an orientation vector.
+ * @param sp_cmd The orientation vector
+ */
+extern void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd);
+
+/** Convert a required airspeed to a certain attitude for the Hybrid.
+ * @param ypr_sp Attitude set-point
+ */
+extern void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp);
+
+/** Description.
+ */
+extern void guidance_hybrid_position_to_airspeed(void);
+
+/** Description.
+ */
+extern void guidance_hybrid_determine_wind_estimate(void);
+
+/** Description.
+ * @param sp_cmd Add Description
+ */
+extern void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd);
+
+/** Description.
+ */
+extern void guidance_hybrid_vertical(void);
+
+
+#endif /* GUIDANCE_HYBRID_H */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 1963337833..9189dc1511 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -28,6 +28,7 @@
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
+#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
@@ -231,7 +232,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
switch (new_mode) {
case GUIDANCE_V_MODE_GUIDED:
case GUIDANCE_V_MODE_HOVER:
- /* disable vertical velocity setpoints */
+ /* disable vertical velocity setpoints */
guidance_v_guided_vel_enabled = false;
/* set current altitude as setpoint */
@@ -327,8 +328,7 @@ void guidance_v_run(bool in_flight)
run_hover_loop(in_flight);
/* update z sp for telemetry/debuging */
guidance_v_z_sp = guidance_v_z_ref;
- }
- else {
+ } else {
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
@@ -366,6 +366,9 @@ void guidance_v_run(bool in_flight)
guidance_v_z_sum_err = 0;
guidance_v_delta_t = nav_throttle;
}
+#if HYBRID_NAVIGATION
+ guidance_hybrid_vertical();
+#else
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
@@ -373,6 +376,7 @@ void guidance_v_run(bool in_flight)
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+#endif
break;
}
@@ -452,6 +456,11 @@ static void run_hover_loop(bool in_flight)
/* feed forward command */
guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff;
+#if HYBRID_NAVIGATION
+ //FIXME: NOT USING FEEDFORWARD COMMAND BECAUSE OF QUADSHOT NAVIGATION
+ guidance_v_ff_cmd = guidance_v_nominal_throttle * MAX_PPRZ;
+#endif
+
/* bound the nominal command to 0.9*MAX_PPRZ */
Bound(guidance_v_ff_cmd, 0, 8640);
diff --git a/sw/airborne/modules/ctrl/gain_scheduling.c b/sw/airborne/modules/ctrl/gain_scheduling.c
index b5b4752999..84ebc3044f 100644
--- a/sw/airborne/modules/ctrl/gain_scheduling.c
+++ b/sw/airborne/modules/ctrl/gain_scheduling.c
@@ -25,10 +25,6 @@
#include "modules/ctrl/gain_scheduling.h"
-//Include for scheduling on transition_status
-#include "firmwares/rotorcraft/guidance/guidance_h.h"
-#include "firmwares/rotorcraft/stabilization.h"
-
// #include "state.h"
#include "math/pprz_algebra_int.h"
diff --git a/sw/airborne/modules/loggers/high_speed_logger_spi_link.c b/sw/airborne/modules/loggers/high_speed_logger_spi_link.c
index 523c1533b6..61f1e1cbbd 100644
--- a/sw/airborne/modules/loggers/high_speed_logger_spi_link.c
+++ b/sw/airborne/modules/loggers/high_speed_logger_spi_link.c
@@ -25,6 +25,7 @@
#include "subsystems/imu.h"
#include "mcu_periph/spi.h"
+
struct high_speed_logger_spi_link_data high_speed_logger_spi_link_data;
struct spi_transaction high_speed_logger_spi_link_transaction;
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index ebe2624e43..bc98b42d89 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit ebe2624e433bf2bf2e73a17d9cb2da2750bddac1
+Subproject commit bc98b42d894c417fb07939c30ec45ec0ee3b02fa
diff --git a/sw/tools/attitude_viz.py b/sw/tools/attitude_viz.py
index 325c5d3b7f..1d233a36af 100755
--- a/sw/tools/attitude_viz.py
+++ b/sw/tools/attitude_viz.py
@@ -338,7 +338,7 @@ def run():
VEHICLE_QUATS = [["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]]
BAR_VALUES = [["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100]]
window_title = "Attitude_Viz"
- rotate_theta = -90
+ rotate_theta = 0
try:
opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"])
for o, a in opts:
@@ -350,7 +350,7 @@ def run():
print(msg)
print("""usage:
-t, --title set window title
--r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: -90)
+-r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: 0)
""")
pygame.init()
screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL | pygame.DOUBLEBUF)