Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[fix] guidance sp was not correct in speed mode (#3236)

Browse Source 
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.
This commit is contained in:
Gautier Hattenberger
2024-02-07 10:20:27 +01:00
committed by GitHub
parent bbcb166d37
commit 19952d8e04
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+6 -3
View File
@@ -474,11 +474,14 @@ void guidance_h_from_nav(bool in_flight)
} else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) { } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) {
guidance_h_guided_run(in_flight); guidance_h_guided_run(in_flight);
} else { } else {
// update carrot for display, even if sp is changed in speed mode // update carrot for GCS display and convert ENU float -> NED int
guidance_h_set_pos(nav.carrot.y, nav.carrot.x); // 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) { switch (nav.setpoint_mode) {
case NAV_SETPOINT_MODE_POS: 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_update_reference();
guidance_h_set_heading(nav.heading); guidance_h_set_heading(nav.heading);
guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h); guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);