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

Improve IO driver feedback

Browse Source
This commit is contained in:
Lorenz Meier
2015-02-08 01:48:41 +01:00
parent 1a6dd7d02e
commit 6eaf634c5d
1 changed files with 11 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+11 -17
View File
@@ -625,14 +625,12 @@ PX4IO::init()
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol == _io_reg_get_error) {
log("failed to communicate with IO");
mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
mavlink_and_console_log_emergency(_mavlink_fd, "Failed to communicate with IO, abort.");
return -1;
}
if (protocol != PX4IO_PROTOCOL_VERSION) {
log("protocol/firmware mismatch");
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
mavlink_and_console_log_emergency(_mavlink_fd, "IO protocol/firmware mismatch, abort.");
return -1;
}
@@ -681,8 +679,7 @@ PX4IO::init()
/* get a status update from IO */
io_get_status();
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
log("INAIR RESTART RECOVERY (needs commander app running)");
mavlink_and_console_log_emergency(_mavlink_fd, "RECOVERING FROM FMU IN-AIR RESTART");
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
@@ -711,7 +708,7 @@ PX4IO::init()
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) {
log("failed to recover from in-air restart (1), aborting IO driver init.");
mavlink_and_console_log_emergency(_mavlink_fd, "Failed to recover from in-air restart (1), abort");
return 1;
}
@@ -767,7 +764,7 @@ PX4IO::init()
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
log("failed to recover from in-air restart (2), aborting IO driver init.");
mavlink_and_console_log_emergency(_mavlink_fd, "Failed to recover from in-air restart (2), abort");
return 1;
}
@@ -809,8 +806,7 @@ PX4IO::init()
ret = io_set_rc_config();
if (ret != OK) {
log("failed to update RC input config");
mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
mavlink_and_console_log_critical(_mavlink_fd, "IO RC config upload fail");
return ret;
}
}
@@ -847,8 +843,6 @@ PX4IO::init()
return -errno;
}
mavlink_log_info(_mavlink_fd, "[IO] init ok");
return OK;
}
@@ -888,7 +882,7 @@ PX4IO::task_main()
(_t_vehicle_control_mode < 0) ||
(_t_param < 0) ||
(_t_vehicle_command < 0)) {
log("subscription(s) failed");
warnx("subscription(s) failed");
goto out;
}
@@ -926,7 +920,7 @@ PX4IO::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
warnx("poll error %d", errno);
continue;
}
@@ -1028,7 +1022,7 @@ PX4IO::task_main()
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
if (pret != OK) {
log("vscale upload failed");
mavlink_and_console_log_critical(_mavlink_fd, "IO vscale upload failed");
}
/* send RC throttle failsafe value to IO */
@@ -1044,7 +1038,7 @@ PX4IO::task_main()
uint16_t failsafe_thr = failsafe_param_val;
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
if (pret != OK) {
log("failsafe upload failed, FS: %d us", (int)failsafe_thr);
mavlink_and_console_log_critical(_mavlink_fd, "failsafe upload failed, FS: %d us", (int)failsafe_thr);
}
}
}
@@ -1355,7 +1349,7 @@ PX4IO::io_set_rc_config()
/* check the IO initialisation flag */
if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
log("config for RC%d rejected by IO", i + 1);
mavlink_and_console_log_critical(_mavlink_fd, "config for RC%d rejected by IO", i + 1);
break;
}