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

Minor state machine improvements and fixes for IO safety / in-air restart handling

Browse Source
This commit is contained in:
Lorenz Meier
2013-06-12 12:30:42 +02:00
parent 1028bd932c
commit eb76d116cc
7 changed files with 78 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+52 -18
View File
@@ -336,6 +336,7 @@ PX4IO::PX4IO() :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_to_safety(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
@@ -389,6 +390,30 @@ PX4IO::init()
*/
_retries = 2;
/* get IO's last seen FMU state */
int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
if (val == _io_reg_get_error) {
mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE");
}
uint16_t arming = val;
/* get basic software version */
/* basic configuration */
usleep(5000);
unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION);
if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) {
mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n",
proto_version,
PX4IO_P_CONFIG_PROTOCOL_VERSION);
mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION);
return 1;
}
/* get some parameters */
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
@@ -414,21 +439,23 @@ PX4IO::init()
* in this case.
*/
uint16_t reg;
printf("arming 0x%04x%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
/* get IO's last seen FMU state */
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
if (ret != OK)
return ret;
/*
* in-air restart is only tried if the IO board reports it is
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
log("INAIR RESTART RECOVERY (needs commander app running)");
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
@@ -482,7 +509,7 @@ PX4IO::init()
cmd.confirmation = 1;
/* send command once */
(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
/* spin here until IO's state has propagated into the system */
do {
@@ -492,16 +519,22 @@ PX4IO::init()
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
}
/* wait 10 ms */
usleep(10000);
/* wait 50 ms */
usleep(50000);
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
if ((hrt_absolute_time() - try_start_time)/1000 > 2000) {
log("failed to recover from in-air restart (2), aborting IO driver init.");
return 1;
}
/* keep waiting for state change for 10 s */
/* re-send if necessary */
if (!status.flag_system_armed) {
orb_publish(ORB_ID(vehicle_command), pub, &cmd);
log("re-sending arm cmd");
}
/* keep waiting for state change for 2 s */
} while (!status.flag_system_armed);
/* regular boot, no in-air restart, init IO */
@@ -540,7 +573,7 @@ PX4IO::init()
return -errno;
}
mavlink_log_info(_mavlink_fd, "[IO] init ok");
mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version);
return OK;
}
@@ -863,14 +896,14 @@ PX4IO::io_handle_status(uint16_t status)
*/
/* check for IO reset - force it back to armed if necessary */
if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the arming flag */
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
_status &= PX4IO_P_STATUS_FLAGS_ARMED;
_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the sync flag */
@@ -891,7 +924,7 @@ PX4IO::io_handle_status(uint16_t status)
struct safety_s safety;
safety.timestamp = hrt_absolute_time();
if (status & PX4IO_P_STATUS_FLAGS_ARMED) {
if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
safety.status = SAFETY_STATUS_UNLOCKED;
} else {
safety.status = SAFETY_STATUS_SAFE;
@@ -1295,7 +1328,8 @@ PX4IO::print_status()
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),