From b9ea500f78e440b40e4316a6a48e3e14aa19ff81 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Fri, 25 Aug 2017 19:05:07 +0200 Subject: [PATCH] Attitude viz message and no_rc takeoff for indi (#2108) --- .../rotorcraft/stabilization/stabilization_indi.c | 2 +- .../stabilization/stabilization_indi_simple.c | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 556bd779ff..a47bd4fc3d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -378,7 +378,7 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con g2_times_du = g2_times_du / INDI_G_SCALING; float v_thrust = 0.0; - if (indi_thrust_increment_set) { + if (indi_thrust_increment_set && in_flight) { v_thrust = indi_thrust_increment; //update thrust command such that the current is correctly estimated diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c index 169c1bb0ba..019046895f 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c @@ -135,6 +135,20 @@ static void send_att_indi(struct transport_tx *trans, struct link_device *dev) &g1_disp.r, &g2_disp); } + +static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Quat *quat = stateGetNedToBodyQuat_i(); + pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID, + &stab_att_sp_quat.qi, + &stab_att_sp_quat.qx, + &stab_att_sp_quat.qy, + &stab_att_sp_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); +} #endif void stabilization_indi_init(void) @@ -144,6 +158,7 @@ void stabilization_indi_init(void) #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat); #endif }