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);