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

px4io: switch to events

Browse Source
This commit is contained in:
Beat Küng 2021-04-29 10:09:40 +02:00 committed by Daniel Agar
parent 7bb123c65e
commit 38ee923658
1 changed files with 39 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/drivers/px4io/px4io.cpp
View File

@ -38,6 +38,7 @@
* PX4IO is connected via DMA enabled high-speed UART.
*/
#include <px4_platform_common/events.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/sem.hpp>
@ -540,7 +541,9 @@ PX4IO::detect()
} else {
PX4_ERR("IO version error");
mavlink_log_emergency(&_mavlink_log_pub, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
mavlink_log_emergency(&_mavlink_log_pub, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!\t");
events::send(events::ID("px4io_io_ver_mismatch"), events::Log::Emergency,
"IO version mismatch, please upgrade the software");
}
return -1;
@ -597,12 +600,16 @@ PX4IO::init()
/* if the error still persists after timing out, we give up */
if (protocol == _io_reg_get_error) {
mavlink_log_emergency(&_mavlink_log_pub, "Failed to communicate with IO, abort.");
mavlink_log_emergency(&_mavlink_log_pub, "Failed to communicate with IO, abort.\t");
events::send(events::ID("px4io_comm_failed"), events::Log::Emergency,
"Failed to communicate with IO, aborting initialization");
return -1;
}
if (protocol != PX4IO_PROTOCOL_VERSION) {
mavlink_log_emergency(&_mavlink_log_pub, "IO protocol/firmware mismatch, abort.");
mavlink_log_emergency(&_mavlink_log_pub, "IO protocol/firmware mismatch, abort.\t");
events::send(events::ID("px4io_proto_fw_mismatch"), events::Log::Emergency,
"IO protocol/firmware mismatch, aborting initialization");
return -1;
}
@ -619,7 +626,9 @@ PX4IO::init()
(_max_rc_input < 1) || (_max_rc_input > 255)) {
PX4_ERR("config read error");
mavlink_log_emergency(&_mavlink_log_pub, "[IO] config read fail, abort.");
mavlink_log_emergency(&_mavlink_log_pub, "[IO] config read fail, abort.\t");
events::send(events::ID("px4io_config_read_failed"), events::Log::Emergency,
"IO config read failed, aborting initialization");
// ask IO to reboot into bootloader as the failure may
// be due to mismatched firmware versions and we want
@ -669,7 +678,9 @@ PX4IO::init()
/* get a status update from IO */
io_get_status();
mavlink_log_emergency(&_mavlink_log_pub, "RECOVERING FROM FMU IN-AIR RESTART");
mavlink_log_emergency(&_mavlink_log_pub, "RECOVERING FROM FMU IN-AIR RESTART\t");
events::send(events::ID("px4io_inair_restart_recovery"), events::Log::Alert,
"Recovering from in-air restart");
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
@ -695,7 +706,9 @@ PX4IO::init()
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) {
mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (1), abort");
mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (1), abort\t");
events::send(events::ID("px4io_inair_restart_recovery_failed"), events::Log::Emergency,
"Failed to recover from in-air restart, aborting initialization");
return 1;
}
@ -725,7 +738,9 @@ PX4IO::init()
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");
mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe\t");
events::send(events::ID("px4io_init_in_failsafe"), events::Log::Emergency,
"IO is in failsafe, forcing flight termination");
/* send command to terminate flight via command API */
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 1.0f; /* request flight termination */
@ -744,7 +759,9 @@ PX4IO::init()
/* 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");
mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort\t");
events::send(events::ID("px4io_inair_restart_recovery_failed3"), events::Log::Emergency,
"Failed to recover from in-air restart, aborting initialization");
return 1;
}
@ -777,7 +794,9 @@ PX4IO::init()
/* 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 (2), abort");
mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (2), abort\t");
events::send(events::ID("px4io_inair_restart_recovery_failed2"), events::Log::Emergency,
"Failed to recover from in-air restart, aborting initialization");
return 1;
}
@ -824,7 +843,9 @@ PX4IO::init()
ret = io_set_rc_config();
if (ret != OK) {
mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail");
mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t");
events::send(events::ID("px4io_io_rc_config_upload_failed"), events::Log::Critical,
"IO RC config upload failed, aborting initialization");
return ret;
}
}
@ -1043,7 +1064,9 @@ PX4IO::task_main()
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
if (pret != OK) {
mavlink_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %" PRId16 " us", failsafe_thr);
mavlink_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %d us\t", (int)failsafe_thr);
events::send<uint16_t>(events::ID("px4io_failsafe_upload_failed"), events::Log::Critical,
"Failsafe upload failed, threshold: {1} us", failsafe_thr);
}
}
}
@ -1792,7 +1815,9 @@ 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)) {
mavlink_log_critical(&_mavlink_log_pub, "config for RC%u rejected by IO", i + 1);
mavlink_log_critical(&_mavlink_log_pub, "config for RC%u rejected by IO\t", i + 1);
events::send<uint16_t>(events::ID("px4io_config_rc_rejected"), events::Log::Error,
"Config for RC{1} rejected by IO", i + 1);
break;
}
@ -2448,7 +2473,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (retries > 0);
if (retries == 0) {
mavlink_log_info(&_mavlink_log_pub, "[IO] mixer upload fail");
mavlink_log_info(&_mavlink_log_pub, "[IO] mixer upload fail\t");
events::send(events::ID("px4io_mixer_upload_failed"), events::Log::Error, "IO mixer upload failed");
/* load must have failed for some reason */
return -EINVAL;