diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index f4e34369a3..f60284f7ba 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -312,7 +312,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa //Calculate roll,pitch and thrust command MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff); - //printf("abi thrust %f\n", euler_cmd.z); AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z); // Coordinated turn @@ -510,7 +509,6 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/ BoundAbs(accel_sp.z, 3.0); - //printf("accel_sp %f %f %f\n", accel_sp.x, accel_sp.y, accel_sp.z); return accel_sp; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index a4d83fb232..5835090549 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -145,6 +145,7 @@ float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT-1] = 1.0}; // variables needed for control float actuator_state_filt_vect[INDI_NUM_ACT]; struct FloatRates angular_accel_ref = {0., 0., 0.}; +struct FloatRates angular_rate_ref = {0., 0., 0.}; float angular_acceleration[3] = {0., 0., 0.}; float actuator_state[INDI_NUM_ACT]; float indi_u[INDI_NUM_ACT]; @@ -233,6 +234,34 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d &(quat->qy), &(quat->qz)); } + +static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev) +{ + struct FloatRates *body_rates = stateGetBodyRates_f(); + struct Int32Vect3 *body_accel_i = stateGetAccelBody_i(); + struct FloatVect3 body_accel_f_telem; + ACCELS_FLOAT_OF_BFP(body_accel_f_telem, *body_accel_i); + float zero = 0; + pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID, + &body_accel_f_telem.x, // input lin.acc + &body_accel_f_telem.y, + &body_accel_f_telem.z, + &body_rates->p, // rate + &body_rates->q, + &body_rates->r, + &angular_rate_ref.p, // rate.sp + &angular_rate_ref.q, + &angular_rate_ref.r, + &angular_acceleration[0], // ang.acc + &angular_acceleration[1], + &angular_acceleration[2], + &angular_accel_ref.p, // ang.acc.sp + &angular_accel_ref.q, + &angular_accel_ref.r, + &zero, &zero, // eff.mat + &zero, &zero, + INDI_NUM_ACT, indi_u); // out +} #endif /** @@ -278,6 +307,7 @@ void stabilization_indi_init(void) #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INDI_G, send_indi_g); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_full_indi); #endif } @@ -616,6 +646,11 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight) rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q; rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r; + // Store for telemetry + angular_rate_ref.p = rate_sp.p; + angular_rate_ref.q = rate_sp.q; + angular_rate_ref.r = rate_sp.r; + // Possibly we can use some bounding here /*BoundAbs(rate_sp.r, 5.0);*/ @@ -846,14 +881,18 @@ void calc_g1g2_pseudo_inv(void) } } -static void rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t UNUSED *rpm_msg, uint8_t UNUSED num_act) +static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act UNUSED) { #if INDI_RPM_FEEDBACK int8_t i; for (i = 0; i < num_act; i++) { - act_obs[i] = (rpm_msg[i].rpm - get_servo_min(i)); - act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i))); - Bound(act_obs[i], 0, MAX_PPRZ); + // Sanity check that index is valid + if (rpm_msg[i].actuator_idx < INDI_NUM_ACT) { + int8_t idx = rpm_msg[i].actuator_idx; + act_obs[idx] = (rpm_msg[i].rpm - get_servo_min(idx)); + act_obs[idx] *= (MAX_PPRZ / (float)(get_servo_max(idx) - get_servo_min(idx))); + Bound(act_obs[idx], 0, MAX_PPRZ); + } } #endif } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c index d5e7817def..663da3d4e0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c @@ -148,18 +148,25 @@ static void send_att_indi(struct transport_tx *trans, struct link_device *dev) struct FloatRates g1_disp; RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE); float g2_disp = indi.est.g2 * INDI_EST_SCALE; + float zero = 0; pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID, - &indi.rate_d[0], + &zero, &zero, &zero, // input lin.acc + &indi.rate[0].o[0], // rate + &indi.rate[1].o[0], + &indi.rate[2].o[0], + &zero, &zero, &zero, // rate.ref + &indi.rate_d[0], // ang.acc = rate.diff &indi.rate_d[1], &indi.rate_d[2], - &indi.angular_accel_ref.p, + &indi.angular_accel_ref.p, // ang.acc.ref &indi.angular_accel_ref.q, &indi.angular_accel_ref.r, - &g1_disp.p, + &g1_disp.p, // matrix &g1_disp.q, &g1_disp.r, - &g2_disp); + &g2_disp, + 0, &zero); // outputs } static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 976ed5280e..04658b50fa 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 976ed5280ec4d55d1b3cde5b5370a45443f66b28 +Subproject commit 04658b50fa9fbd9a00a74647d3657f679eb737b7