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

IO driver - Recover flight termination state from IO after FMU reboot in air

Browse Source
This commit is contained in:
bresch
2019-06-05 16:53:08 +02:00
committed by Beat Küng
parent c75f71abc2
commit 57c2085ce4
1 changed files with 43 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+43 -4
View File
@@ -726,17 +726,56 @@ PX4IO::init()
errx(1, "PRM CMPID");
}
/* send command to arm system via command API */
/* prepare vehicle command */
vehicle_command_s vcmd = {};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 1.0f; /* request arming */
vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
vcmd.target_system = (uint8_t)sys_id;
vcmd.target_component = (uint8_t)comp_id;
vcmd.source_system = (uint8_t)sys_id;
vcmd.source_component = (uint8_t)comp_id;
vcmd.confirmation = true; /* ask to confirm command */
if (reg & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe");
/* send command to terminate flight via command API */
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 1.0f; /* request flight termination */
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
/* send command once */
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
/* spin here until IO's state has propagated into the system */
do {
orb_check(safety_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
}
/* wait 50 ms */
px4_usleep(50000);
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort");
return 1;
}
/* re-send if necessary */
if (!safety.force_failsafe) {
orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
PX4_WARN("re-sending flight termination cmd");
}
/* keep waiting for state change for 2 s */
} while (!safety.force_failsafe);
}
/* send command to arm system via command API */
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 1.0f; /* request arming */
vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
/* send command once */
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);