diff --git a/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp index 89c59f8d82..2915e5683e 100644 --- a/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp +++ b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp @@ -43,6 +43,7 @@ void MulticopterThrowLaunch::update(const bool armed) if (_param_com_throw_en.get()) { if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { _last_velocity = matrix::Vector3f(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); } diff --git a/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp index 973ce3c023..cfcfe3fea3 100644 --- a/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp +++ b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp @@ -56,9 +56,10 @@ public: /** * @return false if feature disabled or already flying */ - bool isThrowLaunchInProgress() const { + bool isThrowLaunchInProgress() const + { return _throw_launch_state != ThrowLaunchState::DISABLED - && _throw_launch_state != ThrowLaunchState::FLYING; + && _throw_launch_state != ThrowLaunchState::FLYING; } bool isReadyToThrow() const { return _throw_launch_state == ThrowLaunchState::ARMED; } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index abc389f082..a14a659549 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -466,10 +466,11 @@ void MulticopterPositionControl::Run() _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get(); } + bool skip_takeoff = _param_com_throw_en.get(); // handle smooth takeoff _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, - _vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample); + _vehicle_constraints.speed_up, skip_takeoff, vehicle_local_position.timestamp_sample); const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 97c4fd2169..9c815c60ed 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -148,6 +148,7 @@ private: // Takeoff / Land (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ + (ParamBool) _param_com_throw_en, /**< throw launch enabled */ (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ (ParamFloat) _param_mpc_tko_speed, (ParamFloat) _param_mpc_land_speed, diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 01278bc002..2e4fbc1f40 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -74,9 +74,6 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co _takeoff_state = TakeoffState::rampup; _takeoff_ramp_progress = 0.f; - } else if (!landed) { - _takeoff_state = TakeoffState::flight; - } else { break; } @@ -106,6 +103,7 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co _takeoff_state = TakeoffState::flight; } + // TODO: need to consider free fall here if (!armed) { _takeoff_state = TakeoffState::disarmed; } diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp index 5b1086c963..d718ef687b 100644 --- a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp +++ b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp @@ -53,15 +53,15 @@ TEST(TakeoffTest, RegularTakeoffRamp) takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); - // armed, landed, don't want takeoff - takeoff.updateTakeoffState(true, true, false, 1.f, false, 500_ms); + // armed, not landed anymore, don't want takeoff + takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); - // armed, landed, don't want takeoff yet, spoolup time passed - takeoff.updateTakeoffState(true, true, false, 1.f, false, 2_s); + // armed, not landed, don't want takeoff yet, spoolup time passed + takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); - // armed, not landed anymore, want takeoff + // armed, not landed, want takeoff takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);