diff options
| author | ilya <ilya@airdog.com> | 2014-11-26 14:24:10 +0200 |
|---|---|---|
| committer | ilya <ilya@airdog.com> | 2014-11-26 14:24:33 +0200 |
| commit | b4123ec053224daa9038a51e1ef6e7790087997f (patch) | |
| tree | 58df93f78a3f69d5a9d792b7620b3d3c90d1b459 | |
| parent | da95642af04b5809a58e96445d042baf16cabd72 (diff) | |
| download | px4-firmware-b4123ec053224daa9038a51e1ef6e7790087997f.tar.gz px4-firmware-b4123ec053224daa9038a51e1ef6e7790087997f.tar.bz2 px4-firmware-b4123ec053224daa9038a51e1ef6e7790087997f.zip | |
RTL precision fixed
| -rw-r--r-- | src/modules/navigator/navigator_mode.cpp | 14 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_mode.h | 2 | ||||
| -rw-r--r-- | src/modules/navigator/rtl.cpp | 2 |
3 files changed, 11 insertions, 7 deletions
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 25ea02a73..ee7cd1b18 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -315,7 +315,7 @@ NavigatorMode::check_current_pos_sp_reached() void -NavigatorMode::land() +NavigatorMode::land(uint8_t reset_setpoint) { pos_sp_triplet = _navigator->get_position_setpoint_triplet(); global_pos = _navigator->get_global_position(); @@ -324,10 +324,14 @@ NavigatorMode::land() pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; - pos_sp_triplet->current.lat = global_pos->lat; - pos_sp_triplet->current.lon = global_pos->lon; - pos_sp_triplet->current.alt = global_pos->alt; - pos_sp_triplet->current.yaw = NAN; + if (reset_setpoint == 1) { + + pos_sp_triplet->current.lat = global_pos->lat; + pos_sp_triplet->current.lon = global_pos->lon; + pos_sp_triplet->current.alt = global_pos->alt; + pos_sp_triplet->current.yaw = NAN; + } + pos_sp_triplet->current.type = SETPOINT_TYPE_LAND; _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 0e84a6881..b90bd35f8 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -103,7 +103,7 @@ public: void updateParamValues(); void updateParamHandles(); - void land(); + void land(uint8_t reset_setpoint = 1); void takeoff(); void disarm(); void resetModeArguments(main_state_t main_state); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7719719cd..ccf58c352 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -207,7 +207,7 @@ RTL::set_rtl_setpoint() break; } case RTL_STATE_LAND: { - land(); + land(0); //do not reset setpoint, let it be at home position break; } case RTL_STATE_LANDED: { |