From 79f9b61aff0570d1ab98dc5a4c7e6a71eec5009b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 May 2013 20:05:20 +0200 Subject: Fixed led patterns to be up to the latest specs --- src/drivers/px4io/px4io.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) (limited to 'src/drivers/px4io/px4io.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e69fbebf7..3006cf885 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -416,7 +416,7 @@ PX4IO::init() * already armed, and has been configured for in-air restart */ if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); @@ -500,7 +500,7 @@ PX4IO::init() /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); @@ -701,11 +701,18 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_ARM_OK; + if (armed.armed && !armed.lockdown) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { - clear |= PX4IO_P_SETUP_ARMING_ARM_OK; + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } + if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { @@ -1271,9 +1278,9 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n", - _battery_amp_per_volt, - _battery_amp_bias, - _battery_mamphour_total); + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); @@ -1303,9 +1310,10 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s\n", + printf("arming 0x%04x%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((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" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -1347,12 +1355,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) switch (cmd) { case PWM_SERVO_ARM: /* set the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); break; case PWM_SERVO_DISARM: /* clear the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; case PWM_SERVO_SET_UPDATE_RATE: -- cgit v1.2.3