fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: send parachute command on termination (#17564)

Browse Source 
* commander: send parachute command on termination

This sends the DO_PARACHUTE command to parachute component.

* commander: fix lying comments and printf

* commander: use one flag for termination triggered

This merges the duplicate flags _flight_termination_triggered and
_flight_flight_termination_printed.

* commander: correct variable name

* commander: always send tune with parachute

* commander: fix target_component for parachute cmd

The previous changes were wrong in that all commands were now sent to
the parachute component which doesn't make any sense. Of course only the
parachute command should be sent there.
This commit is contained in:
Julian Oes 2021-06-15 11:50:30 +02:00 committed by GitHub
parent b550ad22b9
commit e828ba4288
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 45 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/vehicle_command.msg
View File

@ -120,6 +120,10 @@ uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1 # Continuous zoom up/down unt
uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2 # Zoom value as proportion of full camera range
uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3 # Zoom to a focal length
uint8 PARACHUTE_ACTION_DISABLE = 0
uint8 PARACHUTE_ACTION_ENABLE = 1
uint8 PARACHUTE_ACTION_RELEASE = 2
uint8 FAILURE_UNIT_SENSOR_GYRO = 0
uint8 FAILURE_UNIT_SENSOR_ACCEL = 1
uint8 FAILURE_UNIT_SENSOR_MAG = 2

55
src/modules/commander/Commander.cpp
View File

@ -915,6 +915,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd.param1 > 1.5f) {
// Test termination command triggers lockdown but not actual termination.
if (!_lockdown_triggered) {
_armed.lockdown = true;
_lockdown_triggered = true;
@ -922,16 +923,12 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
} else if (cmd.param1 > 0.5f) {
//XXX update state machine?
// Trigger real termination.
if (!_flight_termination_triggered) {
_armed.force_failsafe = true;
_flight_termination_triggered = true;
PX4_WARN("forcing failsafe (termination)");
}
if ((int)cmd.param2 <= 0) {
/* reset all commanded failure modes */
PX4_WARN("reset all non-flighttermination failsafe commands");
send_parachute_command();
}
} else {
@ -2170,9 +2167,15 @@ Commander::run()
case (geofence_result_s::GF_ACTION_TERMINATE) : {
PX4_WARN("Flight termination because of geofence");
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
_armed.force_failsafe = true;
_status_changed = true;
if (!_flight_termination_triggered && !_lockdown_triggered) {
_flight_termination_triggered = true;
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
_armed.force_failsafe = true;
_status_changed = true;
send_parachute_command();
}
break;
}
}
@ -2220,12 +2223,13 @@ Commander::run()
if (_armed.armed && _mission_result_sub.get().flight_termination &&
!_status_flags.circuit_breaker_flight_termination_disabled) {
_armed.force_failsafe = true;
_status_changed = true;
if (!_flight_termination_printed) {
if (!_flight_termination_triggered && !_lockdown_triggered) {
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
_flight_termination_printed = true;
_flight_termination_triggered = true;
_armed.force_failsafe = true;
_status_changed = true;
send_parachute_command();
}
if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
@ -2474,9 +2478,9 @@ Commander::run()
if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH |
vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
const bool is_second_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
if (is_second_after_takeoff && !_lockdown_triggered) {
if (is_right_after_takeoff && !_lockdown_triggered) {
// This handles the case where something fails during the early takeoff phase
_armed.lockdown = true;
_lockdown_triggered = true;
@ -2488,7 +2492,7 @@ Commander::run()
_armed.force_failsafe = true;
_flight_termination_triggered = true;
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
send_parachute_command();
}
}
}
@ -4056,6 +4060,25 @@ void Commander::esc_status_check()
_last_esc_status_updated = esc_status.timestamp;
}
void Commander::send_parachute_command()
{
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
vcmd.source_system = vehicle_status_sub.get().system_id;
vcmd.target_system = vehicle_status_sub.get().system_id;
vcmd.source_component = vehicle_status_sub.get().component_id;
vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
vcmd_pub.publish(vcmd);
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
}
int Commander::print_usage(const char *reason)
{
if (reason) {

3
src/modules/commander/Commander.hpp
View File

@ -187,6 +187,8 @@ private:
bool stabilization_required();
void send_parachute_command();
DEFINE_PARAMETERS(
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@ -373,7 +375,6 @@ private:
bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag
bool _have_taken_off_since_arming{false};
bool _should_set_home_on_takeoff{true};
bool _flight_termination_printed{false};
bool _system_power_usb_connected{false};
cpuload_s _cpuload{};