diff --git a/conf/airframes/tudelft/rotwing_7kg_common.xml b/conf/airframes/tudelft/rotwing_7kg_common.xml
index 62c4cd9170..b677e9ec48 100644
--- a/conf/airframes/tudelft/rotwing_7kg_common.xml
+++ b/conf/airframes/tudelft/rotwing_7kg_common.xml
@@ -196,6 +196,7 @@
+
diff --git a/conf/flight_plans/tudelft/rotwing_EHVB.xml b/conf/flight_plans/tudelft/rotwing_EHVB.xml
index c1c2551c4a..4fd1300717 100644
--- a/conf/flight_plans/tudelft/rotwing_EHVB.xml
+++ b/conf/flight_plans/tudelft/rotwing_EHVB.xml
@@ -23,6 +23,16 @@
+
+
+
+
+
+
+
+
+
+
@@ -36,11 +46,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -48,8 +74,16 @@
+
+
+
+
+
+
+
+
@@ -94,16 +128,15 @@
-
+
-
-
+
-
+
@@ -111,17 +144,17 @@
-
+
-
+
-
-
-
-
-
+
+
+
+
+
-
-
+
+
+
-
+
-
-
-
+
-
+
@@ -180,7 +215,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/rotwing_tharde.xml b/conf/flight_plans/tudelft/rotwing_tharde.xml
index e71fe39720..4aa3d5a3b7 100644
--- a/conf/flight_plans/tudelft/rotwing_tharde.xml
+++ b/conf/flight_plans/tudelft/rotwing_tharde.xml
@@ -19,6 +19,12 @@
+
+
+
+
+
+
@@ -28,15 +34,35 @@
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -56,84 +82,119 @@
+
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
-
+
-
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
-
+
+
+
+
+
+
+
-
-
+
-
+
-
+
-
+
-
+
-
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
-
+
-
+
-
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index b35742abab..dca5dff352 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -670,4 +670,37 @@
settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index bbeabaff91..5f53769386 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -416,6 +416,20 @@ void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed) {
gih_params.max_airspeed = max_airspeed;
}
+void guidance_set_max_bank_angle(float max_bank) {
+ guidance_indi_max_bank = max_bank;
+}
+
+void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd) {
+ gih_params.climb_vspeed_quad = max_climb_speed_quad;
+ gih_params.climb_vspeed_fwd = max_climb_speed_fwd;
+}
+
+void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd) {
+ gih_params.descend_vspeed_quad = max_descend_speed_quad;
+ gih_params.descend_vspeed_fwd = max_descend_speed_fwd;
+}
+
/**
* @param accel_sp accel setpoint in NED frame [m/s^2]
* @param heading_sp the desired heading [rad]
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
index 408e49fc33..19eecc070b 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
@@ -71,6 +71,9 @@ enum GuidanceIndiHybrid_VMode {
extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode);
extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed);
+extern void guidance_set_max_bank_angle(float max_bank);
+extern void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd);
+extern void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd);
struct guidance_indi_hybrid_params {
float pos_gain;
diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.c b/sw/airborne/modules/rotwing_drone/rotwing_state.c
index 1de115a433..1c69f112d3 100644
--- a/sw/airborne/modules/rotwing_drone/rotwing_state.c
+++ b/sw/airborne/modules/rotwing_drone/rotwing_state.c
@@ -60,6 +60,26 @@
#define ROTWING_FW_SKEW_ANGLE 85.0
#endif
+/* Maximum measured skew angle to consider the rotating wing drone in quad (deg) */
+#ifndef ROTWING_QUAD_SKEW_ANGLE
+#define ROTWING_QUAD_SKEW_ANGLE 25.0
+#endif
+
+/* Maximum bank angle while the rotwing is skewing */
+#ifndef ROTWING_TRANSITION_MAX_BANK
+#define ROTWING_TRANSITION_MAX_BANK RadOfDeg(20)
+#endif
+
+/* Maximum climb speed while the rotwing is transitioning */
+#ifndef ROTWING_TRANSITION_MAX_CLIMB_SPEED
+#define ROTWING_TRANSITION_MAX_CLIMB_SPEED 0.5
+#endif
+
+/* Maximum descend speed while the rotwing is transitioning */
+#ifndef ROTWING_TRANSITION_MAX_DESCEND_SPEED
+#define ROTWING_TRANSITION_MAX_DESCEND_SPEED -0.5
+#endif
+
/* TODO: Give a name.... */
#ifndef ROTWING_SKEW_BACK_MARGIN
#define ROTWING_SKEW_BACK_MARGIN 5.0
@@ -80,6 +100,10 @@
#define ROTWING_FW_STALL_TIMEOUT 0.5
#endif
+#ifndef ROTWING_STATE_MIN_FW_DIST
+#define ROTWING_STATE_MIN_FW_DIST 200.0
+#endif
+
/* Fix for not having double can busses */
#ifndef SERVO_BMOTOR_PUSH_IDX
#define SERVO_BMOTOR_PUSH_IDX SERVO_MOTOR_PUSH_IDX
@@ -285,6 +309,18 @@ void rotwing_state_periodic(void)
rotwing_state.max_airspeed = skew_max_airspeed;
}
+ /* Bound max bank angle, climb speed and descend speed if the rotwing drone is transitioning and not in FREE mode */
+ if (rotwing_state.meas_skew_angle_deg < ROTWING_FW_SKEW_ANGLE && rotwing_state.meas_skew_angle_deg > ROTWING_QUAD_SKEW_ANGLE
+ && rotwing_state.state != ROTWING_STATE_FREE) {
+ guidance_set_max_bank_angle(ROTWING_TRANSITION_MAX_BANK);
+ guidance_set_max_climb_speed(ROTWING_TRANSITION_MAX_CLIMB_SPEED, ROTWING_TRANSITION_MAX_CLIMB_SPEED);
+ guidance_set_max_descend_speed(ROTWING_TRANSITION_MAX_DESCEND_SPEED, ROTWING_TRANSITION_MAX_DESCEND_SPEED);
+ } else {
+ guidance_set_max_bank_angle(GUIDANCE_H_MAX_BANK);
+ guidance_set_max_climb_speed(GUIDANCE_INDI_QUAD_CLIMB_SPEED, GUIDANCE_INDI_FWD_CLIMB_SPEED);
+ guidance_set_max_descend_speed(GUIDANCE_INDI_QUAD_DESCEND_SPEED, GUIDANCE_INDI_FWD_DESCEND_SPEED);
+ }
+
// Override failing skewing while fwd
/*if(meas_skew_angle > 70 && rotwing_state_.skewing_failing) {
rotwing_state.min_airspeed = 0; // Vstall + margin
@@ -292,6 +328,7 @@ void rotwing_state_periodic(void)
}*/
guidance_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed);
+
/* Set navigation/guidance settings */
nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew?
@@ -477,44 +514,20 @@ bool rotwing_state_choose_circle_direction(uint8_t wp_id) {
}
}
-void rotwing_state_set_transition_wp(uint8_t wp_id) {
+bool rotwing_state_choose_state_by_dist(uint8_t wp_id) {
+ struct EnuCoor_f wp = waypoints[wp_id].enu_f;
+ float dist2_to_wp = get_dist2_to_point(&wp);
+ float dist_to_wp = sqrtf(dist2_to_wp);
- // Get drone position coordinates in NED
- struct EnuCoor_f target_enu = {.x = stateGetPositionNed_f()->y,
- .y = stateGetPositionNed_f()->x,
- .z = -stateGetPositionNed_f()->z};
-
- static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0};
- struct FloatVect3 x_att_NED;
-
- float_rmat_transp_vmult(&x_att_NED, stateGetNedToBodyRMat_f(), &x_axis);
-
- // set the new transition WP coordinates 100m ahead of the drone
- target_enu.x = 100.f * x_att_NED.y + target_enu.x;
- target_enu.y = 100.f * x_att_NED.x + target_enu.y;
-
- waypoint_set_enu(wp_id, &target_enu);
-
- // Send waypoint update every half second
- RunOnceEvery(100 / 2, {
- // Send to the GCS that the waypoint has been moved
- DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
- &waypoints[wp_id].enu_i.x,
- &waypoints[wp_id].enu_i.y,
- &waypoints[wp_id].enu_i.z);
- });
+ if (autopilot.mode == AP_MODE_NAV) {
+ if (dist_to_wp > ROTWING_STATE_MIN_FW_DIST) {
+ rotwing_state_set(ROTWING_STATE_REQUEST_FW);
+ return true; // Necessary for flight plan
+ } else {
+ rotwing_state_set(ROTWING_STATE_REQUEST_HOVER);
+ return false; // Necessary for flight plan
+ }
+ }
-}
-
-void rotwing_state_update_WP_height(uint8_t wp_id, float height) {
- struct EnuCoor_f target_enu = {.x = waypoints[wp_id].enu_f.x,
- .y = waypoints[wp_id].enu_f.y,
- .z = height};
-
- waypoint_set_enu(wp_id, &target_enu);
-
- DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
- &waypoints[wp_id].enu_i.x,
- &waypoints[wp_id].enu_i.y,
- &waypoints[wp_id].enu_i.z);
+ return false; // Necessary for flight plan
}
diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.h b/sw/airborne/modules/rotwing_drone/rotwing_state.h
index 9d1db31259..70420bb920 100644
--- a/sw/airborne/modules/rotwing_drone/rotwing_state.h
+++ b/sw/airborne/modules/rotwing_drone/rotwing_state.h
@@ -98,7 +98,6 @@ bool rotwing_state_hover_motors_idling(void);
void rotwing_state_set(enum rotwing_states_t state);
bool rotwing_state_choose_circle_direction(uint8_t wp_id);
-void rotwing_state_set_transition_wp(uint8_t wp_id);
-void rotwing_state_update_WP_height(uint8_t wp_id, float height);
+bool rotwing_state_choose_state_by_dist(uint8_t wp_id);
#endif // ROTWING_STATE_H