From 19952d8e0499b8b59162e25e61a8971b0d9b98b9 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 7 Feb 2024 10:20:27 +0100 Subject: [PATCH] [fix] guidance sp was not correct in speed mode (#3236) After #3197, the functions guidance_h_set_xxx are reseting the state so it creates inconsistent behavior when abusing the 'set_pos' to update the gcs display. --- sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 90327600a48..5e0dcc14b29 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -474,11 +474,14 @@ void guidance_h_from_nav(bool in_flight) } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) { guidance_h_guided_run(in_flight); } else { - // update carrot for display, even if sp is changed in speed mode - guidance_h_set_pos(nav.carrot.y, nav.carrot.x); + // update carrot for GCS display and convert ENU float -> NED int + // even if sp is changed later + guidance_h.sp.pos.x = POS_BFP_OF_REAL(nav.carrot.y); + guidance_h.sp.pos.y = POS_BFP_OF_REAL(nav.carrot.x); + switch (nav.setpoint_mode) { case NAV_SETPOINT_MODE_POS: - // set guidance in NED + guidance_h_set_pos(nav.carrot.y, nav.carrot.x); // nav pos is in ENU frame, convert to NED guidance_h_update_reference(); guidance_h_set_heading(nav.heading); guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);