From ec3bc4ee5b1e14629fa5baf9ce55c3eb98db2dd1 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 2 Apr 2019 07:47:39 -0400
Subject: [PATCH] fw_pos_control_l1 reset internal takeoff and landing state
when arming
---
.../FixedwingPositionControl.cpp | 17 +++++++++++++----
.../FixedwingPositionControl.hpp | 2 +-
2 files changed, 14 insertions(+), 5 deletions(-)
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index b3723fd763..6c5763bab4 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -297,12 +297,21 @@ FixedwingPositionControl::parameters_update()
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
- bool updated;
+ bool updated = false;
orb_check(_control_mode_sub, &updated);
if (updated) {
- orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+ const bool was_armed = _control_mode.flag_armed;
+
+ if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) == PX4_OK) {
+
+ // reset state when arming
+ if (!was_armed && _control_mode.flag_armed) {
+ reset_takeoff_state(true);
+ reset_landing_state();
+ }
+ }
}
}
@@ -1870,10 +1879,10 @@ FixedwingPositionControl::run()
}
void
-FixedwingPositionControl::reset_takeoff_state()
+FixedwingPositionControl::reset_takeoff_state(bool force)
{
// only reset takeoff if !armed or just landed
- if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed)) {
+ if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed) || force) {
_runway_takeoff.reset();
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index 72854c83f9..44a9d53784 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -447,7 +447,7 @@ private:
*/
void handle_command();
- void reset_takeoff_state();
+ void reset_takeoff_state(bool force = false);
void reset_landing_state();
/*
--
GitLab