dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ThrowLaunch: check ThrowMode parameter to skip takoff in MulticopterPositionControl 

Signed-off-by: Michał Barciś <mbarcis@mbarcis.net>
This commit is contained in:
Michał Barciś 2023-11-03 17:37:05 +04:00 committed by Matthias Grob
parent 9d455d5f1f
commit 6800a448b0
6 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp
View File

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

5
src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp
View File

@ -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; }

3
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

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

1
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -148,6 +148,7 @@ private:
// Takeoff / Land
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
(ParamBool<px4::params::COM_THROW_EN>) _param_com_throw_en, /**< throw launch enabled */
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,

4
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
View File

@ -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;
}

10
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
View File

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