diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3293cc2b81..e63a7e51cc 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1694,6 +1694,8 @@ void Commander::run() safetyButtonUpdate(); + throwLaunchUpdate(); + vtolStatusUpdate(); _home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed); @@ -1773,7 +1775,8 @@ void Commander::run() _actuator_armed.prearmed = getPrearmState(); _actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed(); _actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION) - || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)); + || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) + || !(_throw_launch_state == ThrowLaunchState::DISABLED || _throw_launch_state == ThrowLaunchState::FLYING)); // _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL _actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION); // _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION @@ -2027,6 +2030,47 @@ void Commander::safetyButtonUpdate() } } +void Commander::throwLaunchUpdate() +{ + if (_param_com_throw_en.get()) { + if (_vehicle_local_position_sub.updated()) { + _vehicle_local_position_sub.copy(&_vehicle_local_position); + } + + if (!isArmed() && _throw_launch_state != ThrowLaunchState::IDLE) { + mavlink_log_info(&_mavlink_log_pub, "The vehicle is DISARMED with throw launch enabled. Do NOT throw it.\t"); + _throw_launch_state = ThrowLaunchState::IDLE; + } + + if (_throw_launch_state == ThrowLaunchState::IDLE && isArmed()) { + mavlink_log_info(&_mavlink_log_pub, "The vehicle is ARMED with throw launch enabled. Throw the vehicle now.\t"); + _throw_launch_state = ThrowLaunchState::ARMED; + } + + float vehicle_speed_squared = ( + _vehicle_local_position.vx * _vehicle_local_position.vx + + _vehicle_local_position.vy * _vehicle_local_position.vy + + _vehicle_local_position.vz * _vehicle_local_position.vz + ); + float min_launch_speed = _param_com_throw_min_speed.get(); + + if (_throw_launch_state == ThrowLaunchState::ARMED && + vehicle_speed_squared >= min_launch_speed * min_launch_speed) { + PX4_INFO("Minimum throw speed exceeded; the motors will start when the vehicle starts falling down."); + _throw_launch_state = ThrowLaunchState::UNSAFE; + } + + if (_throw_launch_state == ThrowLaunchState::UNSAFE && _vehicle_local_position.vz > 0) { + PX4_INFO("Throw successful, starting motors."); + _throw_launch_state = ThrowLaunchState::FLYING; + } + + } else if (_throw_launch_state != ThrowLaunchState::DISABLED) { + // make sure everything is reset when the throw launch is disabled + _throw_launch_state = ThrowLaunchState::DISABLED; + } +} + void Commander::vtolStatusUpdate() { // Make sure that this is only adjusted if vehicle really is of type vtol @@ -2085,6 +2129,9 @@ void Commander::updateTunes() } else if (_vehicle_status.failsafe && isArmed()) { tune_failsafe(true); + } else if (_throw_launch_state == ThrowLaunchState::ARMED) { + set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); + } else { set_tune(tune_control_s::TUNE_ID_STOP); } @@ -2141,7 +2188,10 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } - if (_auto_disarm_landed.get_state()) { + const bool throw_launch_in_progress = (_throw_launch_state == ThrowLaunchState::ARMED + || _throw_launch_state == ThrowLaunchState::UNSAFE); + + if (_auto_disarm_landed.get_state() && !throw_launch_in_progress) { if (_have_taken_off_since_arming) { disarm(arm_disarm_reason_t::auto_disarm_land); @@ -2166,7 +2216,8 @@ void Commander::handleAutoDisarm() if (_actuator_armed.manual_lockdown) { disarm(arm_disarm_reason_t::kill_switch, true); - } else { + } else if (!_param_com_throw_en.get()) { // don't disarm if throw + // launch is enabled disarm(arm_disarm_reason_t::lockdown, true); } } @@ -2298,6 +2349,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_PURPLE; + } else if (_throw_launch_state == ThrowLaunchState::ARMED) { + led_mode = led_control_s::MODE_BLINK_FAST; + led_color = led_control_s::COLOR_YELLOW; + } else if (isArmed()) { led_mode = led_control_s::MODE_ON; set_normal_color = true; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e6f79a1aca..b38e8d8647 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -174,6 +174,8 @@ private: void safetyButtonUpdate(); + void throwLaunchUpdate(); + void vtolStatusUpdate(); void updateTunes(); @@ -201,6 +203,14 @@ private: OFFBOARD_MODE_BIT = (1 << 1), }; + enum class ThrowLaunchState { + DISABLED = 0, + IDLE = 1, + ARMED = 2, + UNSAFE = 3, + FLYING = 4 + }; + /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; @@ -261,7 +271,9 @@ private: bool _arm_tune_played{false}; bool _have_taken_off_since_arming{false}; bool _status_changed{true}; + ThrowLaunchState _throw_launch_state{ThrowLaunchState::DISABLED}; + vehicle_local_position_s _vehicle_local_position{}; vehicle_land_detected_s _vehicle_land_detected{}; // commander publications @@ -277,6 +289,7 @@ private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -328,6 +341,8 @@ private: (ParamInt) _param_com_rc_override, (ParamInt) _param_flight_uuid, (ParamInt) _param_takeoff_finished_action, - (ParamFloat) _param_com_cpu_max + (ParamFloat) _param_com_cpu_max, + (ParamBool) _param_com_throw_en, + (ParamFloat) _param_com_throw_min_speed ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 121fa14236..6503635799 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1134,3 +1134,30 @@ PARAM_DEFINE_INT32(COM_ARMABLE, 1); * @group Commander */ PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f); + +/** + * Enable throw-start + * + * Allows to start the vehicle by throwing it into the air. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_THROW_EN, 0); + +/** + * Minimum speed for the throw start + * + * When the throw launch is enabled, the drone will only arm after this speed is exceeded before detecting + * the freefall. This is a safety feature to ensure the drone does not turn on after accidental drop or + * a rapid movement before the throw. + * + * Set to 0 to disable. + * + * @group Commander + * @min 0 + * @decimal 1 + * @increment 0.1 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 2e4fbc1f40..bc6afcf50a 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -39,6 +39,7 @@ #include #include + void TakeoffHandling::generateInitialRampValue(float velocity_p_gain) { velocity_p_gain = math::max(velocity_p_gain, 0.01f); @@ -74,6 +75,9 @@ 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; } @@ -103,7 +107,6 @@ 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 d718ef687b..5b1086c963 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, not landed anymore, don't want takeoff - takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); + // armed, landed, don't want takeoff + takeoff.updateTakeoffState(true, true, false, 1.f, false, 500_ms); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); - // armed, not landed, don't want takeoff yet, spoolup time passed - takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); + // armed, landed, don't want takeoff yet, spoolup time passed + takeoff.updateTakeoffState(true, true, false, 1.f, false, 2_s); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); - // armed, not landed, want takeoff + // armed, not landed anymore, want takeoff takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);