From 1deced7629e7d140a931c42657f75da512696c7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:09:09 +0200 Subject: Added safety status feedback, disallow arming of a rotary wing with engaged safety --- src/modules/commander/state_machine_helper.c | 35 +++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..ac603abfd 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -65,6 +65,18 @@ static const char *system_state_txt[] = { }; +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status); +} + /** * Transition from one state to another */ @@ -513,6 +525,25 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + + /* reject arming if safety status is wrong */ + if (current_status->flag_safety_present) { + + /* + * The operator should not touch the vehicle if + * its armed, so we don't allow arming in the + * first place + */ + if (is_rotary_wing(current_status)) { + + /* safety is in safe position, disallow arming */ + if (current_status->flag_safety_safe) { + mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + } + + } + } + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } @@ -538,9 +569,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + if (is_rotary_wing(current_status)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; -- cgit v1.2.3 From 8b67f88331a9dc65e5c947da177701317d77f8bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:12:17 +0200 Subject: Play warning tune --- src/modules/commander/state_machine_helper.c | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ac603abfd..e18c0edc3 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -49,6 +49,7 @@ #include #include "state_machine_helper.h" +#include "commander.h" static const char *system_state_txt[] = { "SYSTEM_STATE_PREFLIGHT", @@ -539,6 +540,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* safety is in safe position, disallow arming */ if (current_status->flag_safety_safe) { mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + + /* play warning tune */ + tune_error(); } } -- cgit v1.2.3 From 1028bd932cfd08366dd0dcb8c189ebcf88cce53a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Jun 2013 07:39:12 +0200 Subject: Extended vehicle detection --- src/modules/commander/state_machine_helper.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index e18c0edc3..0a6cfd0b5 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,12 +70,14 @@ bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status); + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } /** -- cgit v1.2.3 From eb76d116cc67c6354c29fa41e49b4cf9df1a472d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:30:42 +0200 Subject: Minor state machine improvements and fixes for IO safety / in-air restart handling --- src/drivers/px4io/px4io.cpp | 70 +++++++++++++++++++++------- src/modules/commander/state_machine_helper.c | 3 ++ src/modules/mavlink/orb_listener.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 11 +++-- src/modules/px4iofirmware/protocol.h | 5 +- src/modules/px4iofirmware/registers.c | 13 +++--- src/modules/px4iofirmware/safety.c | 10 ++-- 7 files changed, 78 insertions(+), 36 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc65c4f25..f033382a6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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, ®, 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" : ""), diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0a6cfd0b5..f42caad57 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -545,6 +545,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* play warning tune */ tune_error(); + + /* abort */ + return; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 295cd5e28..9b2d984f0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -659,7 +659,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 123eb6778..b654bdec4 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -194,7 +194,7 @@ mixer_tick(void) */ bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) @@ -204,11 +204,15 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> armed"); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> disarmed"); } if (mixer_servos_armed) { @@ -263,9 +267,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..497e6af8e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -77,7 +77,7 @@ /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ @@ -93,7 +93,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..a092fc93b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -317,9 +317,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } r_setup_arming = value; @@ -367,9 +369,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a RC config change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..95335f038 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; -- cgit v1.2.3 From 90f5e30f2a177bed2ac08e76699ec3029292d640 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 13:53:26 +0200 Subject: Introduced new actuator_safety topic --- src/drivers/ardrone_interface/ardrone_interface.c | 15 +- src/drivers/blinkm/blinkm.cpp | 21 ++- src/drivers/hil/hil.cpp | 17 ++- src/drivers/mkblctrl/mkblctrl.cpp | 19 +-- src/drivers/px4fmu/fmu.cpp | 27 ++-- src/drivers/px4io/px4io.cpp | 41 ++--- src/modules/commander/commander.c | 170 ++++++++++++--------- src/modules/commander/state_machine_helper.c | 25 +-- src/modules/commander/state_machine_helper.h | 3 +- src/modules/gpio_led/gpio_led.c | 16 +- src/modules/mavlink/orb_listener.c | 18 +-- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 8 +- src/modules/mavlink_onboard/orb_topics.h | 3 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 21 ++- src/modules/sdlog2/sdlog2.c | 23 ++- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/actuator_controls.h | 15 +- src/modules/uORB/topics/actuator_safety.h | 65 ++++++++ src/modules/uORB/topics/vehicle_control_mode.h | 96 ++++++++++++ src/modules/uORB/topics/vehicle_status.h | 5 +- 22 files changed, 424 insertions(+), 193 deletions(-) create mode 100644 src/modules/uORB/topics/actuator_safety.h create mode 100644 src/modules/uORB/topics/vehicle_control_mode.h (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 264041e65..4a6f30a4f 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_armed_s armed; - armed.armed = false; + struct actuator_safety_s safety; + safety.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -322,18 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } else { /* MAIN OPERATION MODE */ - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* for now only spin if armed and immediately shut down * if in failsafe */ - //XXX fix this - //if (armed.armed && !armed.lockdown) { - if (state.flag_fmu_armed) { + if (safety.armed && !safety.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 7a64b72a4..1cfdcfbed 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,7 @@ #include #include #include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,6 +377,7 @@ BlinkM::led() static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +388,7 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_actuator_safety = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +401,9 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +458,14 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct actuator_safety_s actuator_safety; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_actuator_safety; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +479,17 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + + if (new_data_actuator_safety) { + orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); + no_data_actuator_safety = 0; + } else { + no_data_actuator_safety++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -530,7 +549,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_fmu_armed == false) { + if(actuator_safety.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d9aa772d4..e851d8a52 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #include @@ -108,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_safety; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_safety(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -321,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -334,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_safety; fds[1].events = POLLIN; unsigned num_outputs; @@ -426,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); } } ::close(_t_actuators); - ::close(_t_armed); + ::close(_t_safety); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8a..093b53d42 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -132,7 +133,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; unsigned int _motor; int _px4mode; int _frametype; @@ -240,7 +241,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -496,8 +497,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -519,7 +520,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; log("starting"); @@ -625,13 +626,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(as.armed && !as.lockdown); } @@ -639,7 +640,7 @@ MK::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf72892eb..db4c87ddc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -174,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -376,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -396,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -499,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = as.armed && !as.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -535,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1021,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_armed_s aa; + actuator_safety_s as; - aa.armed = true; - aa.lockdown = false; + as.armed = true; + as.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_armed), &aa); + handle = orb_advertise(ORB_ID(actuator_safety), &as); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 962a91c7f..28a3eb917 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -152,7 +153,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic + int _t_actuator_safety; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -327,7 +328,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -433,20 +434,20 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_safety_s armed; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - /* keep checking for an update, ensure we got a recent state, + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); break; } @@ -472,10 +473,10 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - cmd.source_system = status.system_id; - cmd.source_component = status.component_id; + // cmd.target_system = status.system_id; + // cmd.target_component = status.component_id; + // cmd.source_system = status.system_id; + // cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; @@ -484,10 +485,10 @@ PX4IO::init() /* spin here until IO's state has propagated into the system */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); } /* wait 10 ms */ @@ -500,7 +501,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_fmu_armed); + } while (!armed.armed); /* regular boot, no in-air restart, init IO */ } else { @@ -564,8 +565,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -577,7 +578,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -713,16 +714,16 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_armed_s armed; ///< system armed state + actuator_safety_s safety; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (safety.armed && !safety.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c2a7242d1..6812fb1fb 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -73,8 +73,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -137,10 +139,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* Main state machine */ -static struct vehicle_status_s current_status; -static orb_advert_t stat_pub; - /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; @@ -187,7 +185,7 @@ void do_rc_calibration(void); void do_airspeed_calibration(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -304,10 +302,11 @@ void do_rc_calibration() { mavlink_log_info(mavlink_fd, "trim calibration starting"); - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; @@ -718,7 +717,7 @@ void do_airspeed_calibration() close(diff_pres_sub); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -738,13 +737,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -757,14 +756,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -777,7 +776,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -826,7 +825,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -845,7 +844,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -865,7 +864,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -886,7 +885,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -906,7 +905,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -925,7 +924,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -1189,17 +1188,30 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + /* safety topic */ + struct actuator_safety_s safety; + orb_advert_t safety_pub; + /* Initialize safety with all false */ + memset(&safety, 0, sizeof(safety)); + + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_hil_enabled = false; - current_status.flag_fmu_armed = false; - current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1222,16 +1234,18 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; memset(&home, 0, sizeof(home)); - if (stat_pub < 0) { + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); exit(ERROR); @@ -1333,6 +1347,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1386,9 +1401,11 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); + handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); } + + /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -1397,9 +1414,8 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ - if (!current_status.flag_fmu_armed) { + if (!safety.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1417,7 +1433,6 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(current_status.system_id)); param_get(_param_component_id, &(current_status.component_id)); - } } @@ -1564,7 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; // XXX implement this - // state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1579,7 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1694,7 +1709,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_fmu_armed) { + && !safety.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1734,24 +1749,22 @@ int commander_thread_main(int argc, char *argv[]) warnx("mode sw not finite"); /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; - printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; - printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; - printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1811,7 +1824,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1819,9 +1832,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1830,11 +1843,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1850,7 +1863,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1859,9 +1872,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1871,9 +1884,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1885,19 +1898,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1909,10 +1922,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1924,10 +1937,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1975,13 +1988,22 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + // printf("checking\n"); + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + + + printf("System Type: %d\n", current_status.system_type); + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + + } else { + mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + } stick_off_counter = 0; } else { @@ -1993,7 +2015,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; } else { @@ -2095,13 +2117,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_fmu_armed) { + if (sp_offboard.armed && !safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { + } else if (!sp_offboard.armed && safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } } else { @@ -2143,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_preflight_mag_calibration == false && // current_status.flag_preflight_accel_calibration == false) { // /* All ok, no calibration going on, go to standby */ - // do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); // } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); state_changed = false; } diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index daed81553..fea7ee840 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -54,7 +54,7 @@ #include "state_machine_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { int ret = ERROR; @@ -69,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_STANDBY: @@ -81,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -95,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -105,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -114,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_REBOOT: @@ -125,7 +125,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; @@ -139,7 +139,12 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (ret == OK) { current_state->arming_state = new_arming_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + safety->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_safety), safety_pub, safety); } } @@ -460,7 +465,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (ret == OK) { current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 5b57cffb7..824a7529f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,6 +46,7 @@ #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -53,7 +54,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 542821e95..618095d0b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -60,6 +61,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_safety_s safety; + int actuator_safety_sub; bool led_state; int counter; }; @@ -168,11 +171,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + /* select pattern for current status */ int pattern = 0; - /* XXX fmu armed correct? */ - if (priv->status.flag_fmu_armed) { + if (priv->safety.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -181,11 +188,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.arming_state == ARMING_STATE_STANDBY) { + if (priv->safety.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.arming_state == ARMING_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d84406e7a..a4a2ca3e5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_armed_s armed; +struct actuator_safety_s safety; struct mavlink_subscriptions mavlink_subs; @@ -109,7 +109,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); +static void l_actuator_safety(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -133,7 +133,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_actuator_safety, &mavlink_subs.safety_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && safety.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +l_actuator_safety(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); } void @@ -745,8 +745,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index d61cd43dc..e647b090a 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -72,7 +73,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 408a850d8..4b6d81113 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,7 +274,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_fmu_armed) { + if (safety->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -369,7 +369,7 @@ int mavlink_thread_main(int argc, char *argv[]) baudrate = 57600; struct vehicle_status_s v_status; - struct actuator_armed_s armed; + struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -437,7 +437,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f56243..fee5580b3 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -69,7 +70,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372..17c3ba820 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,5 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 3329c5c48..44c2fb1d8 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -84,13 +85,16 @@ static bool motor_test_mode = false; static orb_advert_t actuator_pub; -static struct vehicle_status_s state; + static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ + struct vehicle_status_s state; memset(&state, 0, sizeof(state)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -113,6 +117,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -150,7 +155,7 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_fmu_armed = false; + bool flag_armed = false; bool flag_control_position_enabled = false; bool flag_control_velocity_enabled = false; @@ -200,6 +205,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &state); } + orb_check(safety_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -248,7 +259,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_fmu_armed) { + safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -292,7 +303,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_fmu_armed) { + if (!flag_control_attitude_enabled && safety.armed) { att_sp.yaw_body = att.yaw; } @@ -399,7 +410,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = state.flag_control_manual_enabled; flag_control_position_enabled = state.flag_control_position_enabled; flag_control_velocity_enabled = state.flag_control_velocity_enabled; - flag_fmu_armed = state.flag_fmu_armed; + flag_armed = safety.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8c5ec092d..41c2f67e5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -190,7 +191,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct vehicle_status_s *cmd); +static void handle_status(struct actuator_safety_s *safety); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -623,6 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { + struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -643,6 +645,7 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { + int safety_sub; int cmd_sub; int status_sub; int sensor_sub; @@ -690,6 +693,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- SAFETY STATUS --- */ + subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + fds[fdsc_count].fd = subs.safety_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; @@ -826,6 +835,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -838,7 +848,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf_status); + handle_status(&buf.safety); } handled_topics++; @@ -870,7 +880,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1168,11 +1178,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct vehicle_status_s *status) +void handle_status(struct actuator_safety_s *safety) { - /* XXX fmu armed correct? */ - if (status->flag_fmu_armed != flag_system_armed) { - flag_system_armed = status->flag_fmu_armed; + if (safety->armed != flag_system_armed) { + flag_system_armed = safety->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2..2674354c3 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -137,7 +137,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_safety.h" +ORB_DEFINE(actuator_safety, struct actuator_safety_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..26b967237 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; @@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - #endif \ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h new file mode 100644 index 000000000..b78eb4b7e --- /dev/null +++ b/src/modules/uORB/topics/actuator_safety.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_SAFETY_H +#define TOPIC_ACTUATOR_SAFETY_H + +#include +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_safety_s { + + uint64_t timestamp; + + bool safety_off; /**< Set to true if safety is off */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 000000000..eb35d0884 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ + bool flag_system_emergency; + bool flag_preflight_calibration; + + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_auto_enabled; + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + bool failsave_highlevel; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6d3f8a863..b19075921 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,8 +199,8 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ - bool flag_io_armed; /**< true is motors / actuators of IO are armed */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -245,7 +245,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - }; /** -- cgit v1.2.3 From 5b21362e1ffefe4e28579eb7a853fe5d22288760 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:04:23 +0200 Subject: Arming with IO working now --- src/drivers/px4io/px4io.cpp | 11 +++++------ src/modules/commander/state_machine_helper.c | 4 ++++ 2 files changed, 9 insertions(+), 6 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 28a3eb917..d728b7b76 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -728,12 +728,11 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - // TODO fix this -// if (armed.ready_to_arm) { -// set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } else { -// clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } + if (safety.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; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index fea7ee840..4f2fbc984 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,6 +70,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -82,6 +83,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->condition_system_sensors_initialized) { ret = OK; safety->armed = false; + safety->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -115,6 +117,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -126,6 +129,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; -- cgit v1.2.3 From e556649f2ff6922a7a3b7751b68cdedd0d6254aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:48:41 +0200 Subject: Beep when mode is not possible --- src/modules/commander/commander.c | 3 --- src/modules/commander/state_machine_helper.c | 19 +++++++++++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 6812fb1fb..1d3f90807 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1991,9 +1991,6 @@ int commander_thread_main(int argc, char *argv[]) // printf("checking\n"); if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - - printf("System Type: %d\n", current_status.system_type); if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 4f2fbc984..dcaf775b9 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -198,6 +198,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed first */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed first */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -236,8 +238,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -265,8 +269,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -293,8 +299,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -317,10 +325,13 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -343,6 +354,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -380,8 +392,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -405,6 +419,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a mission ready */ if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -428,8 +443,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -450,8 +467,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; -- cgit v1.2.3 From 9f5565de3221718ba12800a54ca1a0c06b7491ef Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Jun 2013 19:41:54 +0200 Subject: Controllers should not access state machine anymore but access the vehicle_control_mode flags --- src/modules/commander/commander.c | 117 +++++++++--------- src/modules/commander/state_machine_helper.c | 136 +++++++++++---------- src/modules/commander/state_machine_helper.h | 4 +- src/modules/mavlink/orb_topics.h | 1 + src/modules/mavlink_onboard/mavlink.c | 12 +- src/modules/mavlink_onboard/orb_topics.h | 1 + src/modules/mavlink_onboard/util.h | 3 +- .../multirotor_att_control_main.c | 45 +++---- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/actuator_safety.h | 1 + src/modules/uORB/topics/vehicle_control_mode.h | 9 +- src/modules/uORB/topics/vehicle_status.h | 18 +-- 12 files changed, 182 insertions(+), 168 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 1d3f90807..7853d2e79 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1233,6 +1233,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized current_status.condition_system_sensors_initialized = true; + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1240,6 +1243,8 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -1824,7 +1829,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1832,9 +1837,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1843,11 +1848,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1863,7 +1868,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1872,9 +1877,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1884,9 +1889,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1898,19 +1903,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1922,10 +1927,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1937,10 +1942,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -2070,43 +2075,43 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } - - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_positive(); + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_positive(); - } - } + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); + + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index dcaf775b9..394ee67cc 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -161,7 +162,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { int ret = ERROR; @@ -179,11 +180,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; - current_state->flag_control_rates_enabled = false; - current_state->flag_control_attitude_enabled = false; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } break; @@ -201,11 +202,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } } break; @@ -218,11 +219,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } break; @@ -244,11 +245,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -275,11 +276,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -305,11 +306,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -334,11 +335,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -357,11 +358,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -372,11 +373,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } break; @@ -398,11 +399,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -422,11 +423,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -449,11 +450,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -473,11 +474,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -491,6 +492,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->counter++; current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 824a7529f..8d8536090 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -54,8 +55,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index e647b090a..221957d46 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 4b6d81113..dbea2be08 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,18 +274,18 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (safety->hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -296,7 +296,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status->flag_control_velocity_enabled) { + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; @@ -368,7 +368,9 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ @@ -437,7 +439,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index fee5580b3..f01f09e12 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 17c3ba820..c6a2e52bf 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 44c2fb1d8..b4d7fb630 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -91,8 +91,8 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct actuator_safety_s safety; memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; @@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -181,7 +181,6 @@ mc_thread_main(int argc, char *argv[]) } else if (ret == 0) { /* no return value, ignore */ } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -199,10 +198,10 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; - orb_check(state_sub, &updated); + orb_check(control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } orb_check(safety_sub, &updated); @@ -227,9 +226,8 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; @@ -252,13 +250,11 @@ mc_thread_main(int argc, char *argv[]) } - } else if (state.flag_control_manual_enabled) { - - if (state.flag_control_attitude_enabled) { - + } else if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || + if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || + control_mode.flag_control_manual_enabled != flag_control_manual_enabled || safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -266,6 +262,8 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +/* XXX fix this */ +#if 0 if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); @@ -297,6 +295,7 @@ mc_thread_main(int argc, char *argv[]) rc_loss_first_time = false; } else { +#endif rc_loss_first_time = true; att_sp.roll_body = manual.roll; @@ -344,7 +343,9 @@ mc_thread_main(int argc, char *argv[]) att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); +#if 0 } +#endif /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -378,7 +379,7 @@ mc_thread_main(int argc, char *argv[]) } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { + if (control_mode.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -405,11 +406,11 @@ mc_thread_main(int argc, char *argv[]) multirotor_control_rates(&rates_sp, gyro, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_control_position_enabled = state.flag_control_position_enabled; - flag_control_velocity_enabled = state.flag_control_velocity_enabled; + /* update control_mode */ + flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; + flag_control_manual_enabled = control_mode.flag_control_manual_enabled; + flag_control_position_enabled = control_mode.flag_control_position_enabled; + flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; flag_armed = safety.armed; perf_end(mc_loop_perf); @@ -427,7 +428,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); - close(state_sub); + close(control_mode_sub); close(manual_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 2674354c3..313fbf641 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -98,6 +98,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index b78eb4b7e..3a107d41a 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -58,6 +58,7 @@ struct actuator_safety_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ }; ORB_DECLARE(actuator_safety); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index eb35d0884..177e30898 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,16 +61,11 @@ */ struct vehicle_control_mode_s { - /* use of a counter and timestamp recommended (but not necessary) */ - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -83,7 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; + bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ }; /** @@ -91,6 +86,6 @@ struct vehicle_control_mode_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_status); +ORB_DECLARE(vehicle_control_mode); #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b19075921..2bcd57f4b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,18 +199,18 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ + //bool flag_armed; /**< true is motors / actuators are armed */ + //bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ + // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + // bool flag_auto_enabled; + // bool flag_control_rates_enabled; /**< true if rates are stabilized */ + // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + // bool flag_control_position_enabled; /**< true if position is controlled */ // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ -- cgit v1.2.3 From 0ecc9c4bf4f2bf9fe1d99b5cbdf398718d2dccdd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 16:30:35 +0200 Subject: Shrinking the main commander file a bit --- src/modules/commander/accelerometer_calibration.c | 14 +- src/modules/commander/accelerometer_calibration.h | 1 - src/modules/commander/airspeed_calibration.c | 111 +++++ src/modules/commander/airspeed_calibration.h | 46 ++ src/modules/commander/baro_calibration.c | 54 +++ src/modules/commander/baro_calibration.h | 46 ++ src/modules/commander/calibration_routines.c | 1 + src/modules/commander/commander.c | 513 ++-------------------- src/modules/commander/commander.h | 4 - src/modules/commander/commander_helper.c | 129 ++++++ src/modules/commander/commander_helper.h | 66 +++ src/modules/commander/gyro_calibration.c | 151 +++++++ src/modules/commander/gyro_calibration.h | 46 ++ src/modules/commander/mag_calibration.c | 278 ++++++++++++ src/modules/commander/mag_calibration.h | 46 ++ src/modules/commander/module.mk | 8 +- src/modules/commander/rc_calibration.c | 83 ++++ src/modules/commander/rc_calibration.h | 46 ++ src/modules/commander/state_machine_helper.c | 68 +-- src/modules/commander/state_machine_helper.h | 12 +- 20 files changed, 1159 insertions(+), 564 deletions(-) create mode 100644 src/modules/commander/airspeed_calibration.c create mode 100644 src/modules/commander/airspeed_calibration.h create mode 100644 src/modules/commander/baro_calibration.c create mode 100644 src/modules/commander/baro_calibration.h create mode 100644 src/modules/commander/commander_helper.c create mode 100644 src/modules/commander/commander_helper.h create mode 100644 src/modules/commander/gyro_calibration.c create mode 100644 src/modules/commander/gyro_calibration.h create mode 100644 src/modules/commander/mag_calibration.c create mode 100644 src/modules/commander/mag_calibration.h create mode 100644 src/modules/commander/rc_calibration.c create mode 100644 src/modules/commander/rc_calibration.h (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 231739962..7bae8ad40 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -70,15 +70,25 @@ */ #include "accelerometer_calibration.h" +#include "commander_helper.h" +#include +#include #include +#include +#include +#include +#include + + #include #include #include #include +#include +#include #include -void do_accel_calibration(int mavlink_fd); int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -355,7 +365,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) + if (det == 0.0f) return ERROR; // Singular matrix dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 6a920c4ef..4175a25f4 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -9,7 +9,6 @@ #define ACCELEROMETER_CALIBRATION_H_ #include -#include void do_accel_calibration(int mavlink_fd); diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c new file mode 100644 index 000000000..feaf11aee --- /dev/null +++ b/src/modules/commander/airspeed_calibration.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed_calibration.c + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h new file mode 100644 index 000000000..92f5651ec --- /dev/null +++ b/src/modules/commander/airspeed_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.h + * Airspeed sensor calibration routine + */ + +#ifndef AIRSPEED_CALIBRATION_H_ +#define AIRSPEED_CALIBRATION_H_ + +#include + +void do_airspeed_calibration(int mavlink_fd); + +#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c new file mode 100644 index 000000000..a70594794 --- /dev/null +++ b/src/modules/commander/baro_calibration.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file baro_calibration.c + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +void do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return; +} diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h new file mode 100644 index 000000000..ac0f4be36 --- /dev/null +++ b/src/modules/commander/baro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.h + * Barometer calibration routine + */ + +#ifndef BARO_CALIBRATION_H_ +#define BARO_CALIBRATION_H_ + +#include + +void do_baro_calibration(int mavlink_fd); + +#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c index a26938637..799cd4269 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.c @@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4a5eb23ad..41baa34d5 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -91,15 +91,15 @@ #include #include +#include "commander_helper.h" #include "state_machine_helper.h" - -#include -#include -#include -#include - #include "calibration_routines.h" #include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ @@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); extern struct system_load_s system_load; +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -130,7 +133,6 @@ extern struct system_load_s system_load; /* File descriptors */ static int leds; -static int buzzer; static int mavlink_fd; /* flags */ @@ -167,27 +169,17 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int buzzer_init(void); -void buzzer_deinit(void); int led_init(void); void led_deinit(void); int led_toggle(int led); int led_on(int led); int led_off(int led); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); void do_reboot(void); -void do_gyro_calibration(void); -void do_mag_calibration(void); -void do_rc_calibration(void); -void do_airspeed_calibration(void); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); +// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -206,23 +198,6 @@ void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void buzzer_deinit() -{ - close(buzzer); -} - int led_init() { @@ -268,27 +243,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, 2); -} - -void tune_positive() -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_neutral() -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void tune_negative() -{ - ioctl(buzzer, TONE_SET_ALARM, 5); -} - void do_reboot() { mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); @@ -298,424 +252,6 @@ void do_reboot() } -void do_rc_calibration() -{ - mavlink_log_info(mavlink_fd, "trim calibration starting"); - - /* XXX fix this */ - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - tune_positive(); - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration() -{ - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_neutral(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - close(sub_mag); -} - -void do_gyro_calibration() -{ - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - - -void do_airspeed_calibration() -{ - /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_positive(); - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - close(diff_pres_sub); -} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { @@ -1182,7 +718,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Failed to initialize leds"); } - if (buzzer_init() != 0) { + if (buzzer_init() != OK) { warnx("ERROR: Failed to initialize buzzer"); } @@ -1246,7 +782,12 @@ int commander_thread_main(int argc, char *argv[]) /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); @@ -1715,7 +1256,7 @@ int commander_thread_main(int argc, char *argv[]) // state_changed = true; // } - orb_check(ORB_ID(vehicle_gps_position), &new_data); + orb_check(gps_sub, &new_data); if (new_data) { @@ -2222,18 +1763,18 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + if (tune_arm() == OK) arm_tune_played = true; /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { - ioctl(buzzer, TONE_SET_ALARM, 0); + tune_stop(); battery_tune_played = false; } @@ -2305,25 +1846,25 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_GYRO_CALIBRATION: - do_gyro_calibration(); + do_gyro_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: - do_mag_calibration(); + do_mag_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - // do_baro_calibration(); + // do_baro_calibration(mavlink_fd); case LOW_PRIO_TASK_RC_CALIBRATION: - // do_rc_calibration(); + // do_rc_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; @@ -2337,7 +1878,7 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - do_airspeed_calibration(); + do_airspeed_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h index 04b4e72ab..6e57c0ba5 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander.h @@ -49,10 +49,6 @@ #ifndef COMMANDER_H_ #define COMMANDER_H_ -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f -void tune_confirm(void); -void tune_error(void); #endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c new file mode 100644 index 000000000..a75b5dec3 --- /dev/null +++ b/src/modules/commander/commander_helper.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_helper.c + * Commander helper functions implementations + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "commander_helper.h" + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, 12); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 14); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, 0); +} + diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h new file mode 100644 index 000000000..ea96d72a6 --- /dev/null +++ b/src/modules/commander/commander_helper.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_helper.h + * Commander helper functions definitions + */ + +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ + +#include +#include +#include +#include + + +bool is_multirotor(const struct vehicle_status_s *current_status); +bool is_rotary_wing(const struct vehicle_status_s *current_status); + +int buzzer_init(void); +void buzzer_deinit(void); + +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +int tune_arm(void); +int tune_critical_bat(void); +int tune_low_bat(void); + +void tune_stop(void); + +#endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c new file mode 100644 index 000000000..f452910c9 --- /dev/null +++ b/src/modules/commander/gyro_calibration.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.c + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h new file mode 100644 index 000000000..cd262507d --- /dev/null +++ b/src/modules/commander/gyro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.h + * Gyroscope calibration routine + */ + +#ifndef GYRO_CALIBRATION_H_ +#define GYRO_CALIBRATION_H_ + +#include + +void do_gyro_calibration(int mavlink_fd); + +#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c new file mode 100644 index 000000000..dbd31a7e7 --- /dev/null +++ b/src/modules/commander/mag_calibration.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.c + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h new file mode 100644 index 000000000..fd2085f14 --- /dev/null +++ b/src/modules/commander/mag_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.h + * Magnetometer calibration routine + */ + +#ifndef MAG_CALIBRATION_H_ +#define MAG_CALIBRATION_H_ + +#include + +void do_mag_calibration(int mavlink_fd); + +#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fe44e955a..fef8e366b 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -38,6 +38,12 @@ MODULE_COMMAND = commander SRCS = commander.c \ state_machine_helper.c \ + commander_helper.c \ calibration_routines.c \ - accelerometer_calibration.c + accelerometer_calibration.c \ + gyro_calibration.c \ + mag_calibration.c \ + baro_calibration.c \ + rc_calibration.c \ + airspeed_calibration.c diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c new file mode 100644 index 000000000..9aa6b86fe --- /dev/null +++ b/src/modules/commander/rc_calibration.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_calibration.c + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include + + +void do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} \ No newline at end of file diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h new file mode 100644 index 000000000..6505c7364 --- /dev/null +++ b/src/modules/commander/rc_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_calibration.h + * Remote Control calibration routine + */ + +#ifndef RC_CALIBRATION_H_ +#define RC_CALIBRATION_H_ + +#include + +void do_rc_calibration(int mavlink_fd); + +#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 87aad6270..c15fc91a0 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -54,21 +54,8 @@ #include #include "state_machine_helper.h" -#include "commander.h" +#include "commander_helper.h" -bool is_multirotor(const struct vehicle_status_s *current_status) -{ - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); -} - -bool is_rotary_wing(const struct vehicle_status_s *current_status) -{ - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); -} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { @@ -568,7 +555,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (valid_transition) { current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); @@ -579,50 +570,6 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ -// if (current_status->flag_control_rates_enabled) { -// current_status->onboard_control_sensors_present |= 0x400; -// -// } else { -// current_status->onboard_control_sensors_present &= ~0x400; -// } - -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; -// -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -} - -// void publish_armed_status(const struct vehicle_status_s *current_status) -// { -// struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; -// -// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ -// /* XXX allow arming only if core sensors are ok */ -// armed.ready_to_arm = true; -// -// /* lock down actuators if required, only in HIL */ -// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; -// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); -// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -// } - - // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status // */ @@ -805,3 +752,4 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b015c4efe..b553a4b56 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -50,15 +50,7 @@ #include -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -bool is_multirotor(const struct vehicle_status_s *current_status); - -bool is_rotary_wing(const struct vehicle_status_s *current_status); - -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -- cgit v1.2.3 From 1b38cf715d85b15f2200d49b64fbe22a05b71937 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 15 Jul 2013 22:15:15 +0200 Subject: Renamed actuator_safety back to actuator_armed, compiling but untested --- src/drivers/ardrone_interface/ardrone_interface.c | 19 +++-- src/drivers/blinkm/blinkm.cpp | 83 ++++++++++++--------- src/drivers/hil/hil.cpp | 18 ++--- src/drivers/mkblctrl/mkblctrl.cpp | 20 ++--- src/drivers/px4fmu/fmu.cpp | 28 +++---- src/drivers/px4io/px4io.cpp | 37 ++++----- .../flow_position_control_main.c | 10 +-- .../flow_position_estimator_main.c | 20 ++--- .../flow_speed_control/flow_speed_control_main.c | 12 +-- src/modules/commander/commander.c | 87 +++++++++++----------- src/modules/commander/commander_helper.h | 2 +- src/modules/commander/state_machine_helper.c | 26 +++---- src/modules/commander/state_machine_helper.h | 4 +- src/modules/gpio_led/gpio_led.c | 14 ++-- src/modules/mavlink/orb_listener.c | 18 ++--- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 10 +-- src/modules/mavlink_onboard/orb_topics.h | 2 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 18 ++--- src/modules/sdlog2/sdlog2.c | 32 ++++---- src/modules/uORB/objects_common.cpp | 9 ++- src/modules/uORB/topics/actuator_armed.h | 58 +++++++++++++++ src/modules/uORB/topics/actuator_safety.h | 66 ---------------- src/modules/uORB/topics/safety.h | 57 ++++++++++++++ src/modules/uORB/topics/vehicle_control_mode.h | 6 +- 26 files changed, 367 insertions(+), 294 deletions(-) create mode 100644 src/modules/uORB/topics/actuator_armed.h delete mode 100644 src/modules/uORB/topics/actuator_safety.h create mode 100644 src/modules/uORB/topics/safety.h (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 187820372..b88f61ce8 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -53,9 +53,10 @@ #include #include #include -#include +#include #include -#include +#include +#include #include @@ -244,17 +245,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int led_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_safety_s safety; - safety.armed = false; + struct actuator_armed_s armed; + //XXX is this necessairy? + armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -325,12 +324,12 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* for now only spin if armed and immediately shut down * if in failsafe */ - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index a11f88477..e83a87aa9 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,7 +116,8 @@ #include #include #include -#include +#include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,8 +377,9 @@ BlinkM::led() { static int vehicle_status_sub_fd; + static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; - static int actuator_safety_sub_fd; + static int actuator_armed_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -388,7 +390,8 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; - static int no_data_actuator_safety = 0; + static int no_data_vehicle_control_mode = 0; + static int no_data_actuator_armed = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -401,8 +404,11 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); - actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 1000); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -458,14 +464,16 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; - struct actuator_safety_s actuator_safety; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; - bool new_data_actuator_safety; + bool new_data_vehicle_control_mode; + bool new_data_actuator_armed; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -479,15 +487,26 @@ BlinkM::led() no_data_vehicle_status = 3; } - orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); - if (new_data_actuator_safety) { - orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); - no_data_actuator_safety = 0; + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; } else { - no_data_actuator_safety++; - if(no_data_vehicle_status >= 3) - no_data_vehicle_status = 3; + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); @@ -549,7 +568,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(actuator_safety.armed == false) { + if(actuator_armed.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -573,25 +592,21 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - /* handle 4th led - flightmode indicator */ -#warning fix this -// switch((int)vehicle_status_raw.flight_mode) { -// case VEHICLE_FLIGHT_MODE_MANUAL: -// led_color_4 = LED_AMBER; -// break; -// -// case VEHICLE_FLIGHT_MODE_STAB: -// led_color_4 = LED_YELLOW; -// break; -// -// case VEHICLE_FLIGHT_MODE_HOLD: -// led_color_4 = LED_BLUE; -// break; -// -// case VEHICLE_FLIGHT_MODE_AUTO: -// led_color_4 = LED_GREEN; -// break; -// } + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + + //XXX please check + if (vehicle_control_mode.flag_control_position_enabled) + led_color_4 = LED_GREEN; + else if (vehicle_control_mode.flag_control_velocity_enabled) + led_color_4 = LED_BLUE; + else if (vehicle_control_mode.flag_control_attitude_enabled) + led_color_4 = LED_YELLOW; + else if (vehicle_control_mode.flag_control_manual_enabled) + led_color_4 = LED_AMBER; + else + led_color_4 = LED_OFF; + + } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat�s */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 15fbf8c88..3e30e3292 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,7 +75,7 @@ #include #include -#include +#include #include #include @@ -109,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_safety; + int _t_armed; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -162,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_safety(-1), + _t_armed(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -322,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_safety, 200); /* 5Hz update rate */ + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -335,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_safety; + fds[1].fd = _t_armed; fds[1].events = POLLIN; unsigned num_outputs; @@ -427,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s aa; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); } } ::close(_t_actuators); - ::close(_t_safety); + ::close(_t_armed); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3fc1054e6..06930db38 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,7 +74,7 @@ #include #include #include -#include +#include #include #include @@ -135,7 +135,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; unsigned int _motor; int _px4mode; int _frametype; @@ -248,7 +248,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _t_esc_status(0), @@ -513,8 +513,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -540,7 +540,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; log("starting"); @@ -651,13 +651,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(as.armed && !as.lockdown); + mk_servo_arm(aa.armed && !aa.lockdown); } @@ -700,7 +700,7 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d0499d2fd..41c8c8bb7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,7 @@ #include #include #include -#include +#include #include #include @@ -105,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -175,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -377,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -397,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -500,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = as.armed && !as.lockdown; + bool set_armed = aa.armed && !aa.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -536,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1022,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_safety_s as; + actuator_armed_s aa; - as.armed = true; - as.lockdown = false; + aa.armed = true; + aa.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_safety), &as); + handle = orb_advertise(ORB_ID(actuator_armed), &aa); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 94f231821..ea732eccd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,7 +75,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -178,7 +179,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_actuator_safety; ///< system armed control topic + int _t_actuator_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -360,7 +361,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -505,9 +506,9 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int safety_sub = orb_subscribe(ORB_ID(actuator_armed)); /* fill with initial values, clear updated flag */ - struct actuator_safety_s safety; + struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; @@ -518,7 +519,7 @@ PX4IO::init() if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); break; } @@ -559,7 +560,7 @@ PX4IO::init() orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); } /* wait 50 ms */ @@ -643,8 +644,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -656,7 +657,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -832,21 +833,21 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_safety_s safety; ///< system armed state + actuator_armed_s armed; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (safety.ready_to_arm) { + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1004,10 +1005,10 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct actuator_safety_s safety; + struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(safety), _to_safety, &safety); if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; @@ -1019,9 +1020,9 @@ PX4IO::io_handle_status(uint16_t status) /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); + orb_publish(ORB_ID(safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); + _to_safety = orb_advertise(ORB_ID(safety), &safety); } return ret; diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 0b9404582..621d3253f 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,7 +159,7 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; @@ -171,7 +171,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); @@ -261,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -578,7 +578,7 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 557fdf37c..5e80066a7 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,8 +159,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; @@ -173,8 +173,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + /* subscribe to armed topic */ + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* subscribe to safety topic */ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -270,7 +270,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ @@ -284,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -356,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !safety.armed) + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; @@ -453,7 +453,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 8032a6264..6af955cd7 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -156,7 +156,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -166,7 +166,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -229,8 +229,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the safety topic */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the armed topic */ + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of the control mode */ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ @@ -355,7 +355,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(att_sp_pub); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 2b0b3a415..c4ee605cc 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -76,9 +76,10 @@ #include #include #include -#include +#include #include #include +#include #include #include @@ -177,7 +178,7 @@ int led_off(int led); void do_reboot(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); // int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -253,7 +254,7 @@ void do_reboot() -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -273,13 +274,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -292,14 +293,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -312,7 +313,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -361,7 +362,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -380,7 +381,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -400,7 +401,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -421,7 +422,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -441,7 +442,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -460,7 +461,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -734,12 +735,11 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - /* safety topic */ - struct actuator_safety_s safety; - orb_advert_t safety_pub; - /* Initialize safety with all false */ - memset(&safety, 0, sizeof(safety)); - + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); /* flags for control apps */ struct vehicle_control_mode_s control_mode; @@ -768,8 +768,8 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; @@ -789,10 +789,7 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); - - /* but also subscribe to it */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -844,6 +841,11 @@ int commander_thread_main(int argc, char *argv[]) /* XXX what exactly is this for? */ uint64_t last_print_time = 0; + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -917,7 +919,6 @@ int commander_thread_main(int argc, char *argv[]) /* XXX use this! */ //uint64_t failsave_ll_start_time = 0; - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -936,7 +937,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -990,7 +991,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); + handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } @@ -1003,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1028,7 +1029,7 @@ int commander_thread_main(int argc, char *argv[]) orb_check(safety_sub, &new_data); if (new_data) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ @@ -1170,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1287,7 +1288,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !safety.armed) { + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1579,7 +1580,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); tune_positive(); } @@ -1595,13 +1596,12 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position --> arm */ - if (current_status.flag_control_manual_enabled && - current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + /* check if left stick is in lower right position and we're in manual --> arm */ + if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; tune_positive(); @@ -1704,13 +1704,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !safety.armed) { + if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - } else if (!sp_offboard.armed && safety.armed) { + } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } } else { @@ -1772,7 +1772,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; @@ -1789,7 +1789,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1812,6 +1812,7 @@ int commander_thread_main(int argc, char *argv[]) close(sp_offboard_sub); close(global_position_sub); close(sensor_sub); + close(safety_sub); close(cmd_sub); warnx("exiting"); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index ea96d72a6..b06e85169 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -43,7 +43,7 @@ #include #include -#include +#include #include diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index c15fc91a0..0b241d108 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -57,7 +57,7 @@ #include "commander_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { int ret = ERROR; @@ -73,8 +73,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -86,8 +86,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - safety->armed = false; - safety->ready_to_arm = true; + armed->armed = false; + armed->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -101,7 +101,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -111,7 +111,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -120,8 +120,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -132,8 +132,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; @@ -151,8 +151,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - safety->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_safety), safety_pub, safety); + armed->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_armed), armed_pub, armed); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b553a4b56..e591d2fef 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,11 +46,11 @@ #include #include -#include +#include #include -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 97b585cb7..6eb425705 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -63,8 +63,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; - struct actuator_safety_s safety; - int actuator_safety_sub; + struct actuator_armed_s armed; + int actuator_armed_sub; bool led_state; int counter; }; @@ -233,12 +233,12 @@ void gpio_led_cycle(FAR void *arg) orb_check(priv->vehicle_status_sub, &status_updated); if (status_updated) - orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); /* select pattern for current status */ int pattern = 0; - if (priv->safety.armed) { + if (priv->armed.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -247,10 +247,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->safety.ready_to_arm) { + if (priv->armed.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 95bc8fdc0..57a5d5dd5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_safety_s safety; +struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_0; struct vehicle_attitude_s att; @@ -110,7 +110,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_safety(const struct listener *l); +static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -135,7 +135,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_safety, &mavlink_subs.safety_sub, 0}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && safety.armed) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_safety(const struct listener *l) +l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void @@ -759,8 +759,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 3b968b911..506f73105 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -60,7 +60,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,6 +79,7 @@ struct mavlink_subscriptions { int man_control_sp_sub; int safety_sub; int actuators_sub; + int armed_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 2d5a64ee8..9ed2c6c12 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -273,7 +273,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -284,12 +284,12 @@ get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, co *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (safety->hil_enabled) { + if (control_mode->flag_system_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (safety->armed) { + if (armed->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -370,7 +370,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* XXX this is never written? */ struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; - struct actuator_safety_s safety; + struct actuator_armed_s armed; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -438,7 +438,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f01f09e12..1b49c9ce4 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index c6a2e52bf..c84b6fd26 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -51,5 +51,5 @@ extern volatile bool thread_should_exit; * Translate the custom state into standard mavlink modes and state. */ extern void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 70a61fc4e..20502c0ea 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -94,8 +94,8 @@ mc_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -121,7 +121,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -210,10 +210,10 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(safety_sub, &updated); + orb_check(armed_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* get a local copy of manual setpoint */ @@ -261,7 +261,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || control_mode.flag_control_manual_enabled != flag_control_manual_enabled || - safety.armed != flag_armed) { + armed.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -275,7 +275,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + if (!flag_control_attitude_enabled && armed.armed) { att_sp.yaw_body = att.yaw; } @@ -366,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = control_mode.flag_control_manual_enabled; flag_control_position_enabled = control_mode.flag_control_position_enabled; flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; - flag_armed = safety.armed; + flag_armed = armed.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 80ee3adee..b20d38b5e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,7 +60,7 @@ #include #include -#include +#include #include #include #include @@ -194,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct actuator_safety_s *safety); +static void handle_status(struct actuator_armed_s *armed); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -628,8 +628,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); - struct actuator_safety_s buf_safety; - memset(&buf_safety, 0, sizeof(buf_safety)); + struct actuator_armed_s buf_armed; + memset(&buf_armed, 0, sizeof(buf_armed)); /* warning! using union here to save memory, elements should be used separately! */ union { @@ -659,7 +659,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int safety_sub; + int armed_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -717,9 +717,9 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY --- */ - subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - fds[fdsc_count].fd = subs.safety_sub; + /* --- ACTUATOR ARMED --- */ + subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + fds[fdsc_count].fd = subs.armed_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -896,12 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- SAFETY- LOG MANAGEMENT --- */ + /* --- ARMED- LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); + orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed); if (log_when_armed) { - handle_status(&buf_safety); + handle_status(&buf_armed); } handled_topics++; @@ -912,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); //if (log_when_armed) { - // handle_status(&buf_safety); + // handle_status(&buf_armed); //} //handled_topics++; @@ -944,7 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1348,10 +1348,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct actuator_safety_s *safety) +void handle_status(struct actuator_armed_s *armed) { - if (safety->armed != flag_system_armed) { - flag_system_armed = safety->armed; + if (armed->armed != flag_system_armed) { + flag_system_armed = armed->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a061fe676..ed77bb7ef 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); + #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); @@ -153,8 +156,8 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -#include "topics/actuator_safety.h" -ORB_DEFINE(actuator_safety, struct actuator_safety_s); +#include "topics/actuator_armed.h" +ORB_DEFINE(actuator_armed, struct actuator_armed_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h new file mode 100644 index 000000000..6e944ffee --- /dev/null +++ b/src/modules/uORB/topics/actuator_armed.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_armed.h + * + * Actuator armed topic + * + */ + +#ifndef TOPIC_ACTUATOR_ARMED_H +#define TOPIC_ACTUATOR_ARMED_H + +#include +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_armed_s { + + uint64_t timestamp; + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_armed); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h deleted file mode 100644 index c431217ab..000000000 --- a/src/modules/uORB/topics/actuator_safety.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file actuator_controls.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller - */ - -#ifndef TOPIC_ACTUATOR_SAFETY_H -#define TOPIC_ACTUATOR_SAFETY_H - -#include -#include "../uORB.h" - -/** global 'actuator output is live' control. */ -struct actuator_safety_s { - - uint64_t timestamp; - bool safety_switch_available; /**< Set to true if a safety switch is connected */ - bool safety_off; /**< Set to true if safety is off */ - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ -}; - -ORB_DECLARE(actuator_safety); - -#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 000000000..a5d21cd4a --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file safety.h + * + * Safety topic to pass safety state from px4io driver to commander + * This concerns only the safety button of the px4io but has nothing to do + * with arming/disarming. + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +struct safety_s { + + uint64_t timestamp; + bool safety_switch_available; /**< Set to true if a safety switch is connected */ + bool safety_off; /**< Set to true if safety is off */ +}; + +ORB_DECLARE(safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 177e30898..02bf5568a 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -69,9 +69,13 @@ struct vehicle_control_mode_s bool flag_system_emergency; bool flag_preflight_calibration; + // XXX needs yet to be set by state machine helper + bool flag_system_hil_enabled; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; + // XXX shouldn't be necessairy + //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ -- cgit v1.2.3 From bcdedd9a35a5b9ebf3851a0d472adab8d3e7edac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 18:55:32 +0200 Subject: Changed location of lots of flags and conditions, needs testing and more work --- src/drivers/blinkm/blinkm.cpp | 2 +- src/drivers/px4io/px4io.cpp | 18 +-- .../attitude_estimator_ekf_main.cpp | 14 +-- .../attitude_estimator_so3_comp_main.cpp | 14 +-- src/modules/commander/commander.c | 122 ++++++++++----------- src/modules/commander/state_machine_helper.c | 11 +- src/modules/commander/state_machine_helper.h | 2 +- src/modules/controllib/fixedwing.cpp | 24 ++-- .../fixedwing_backside/fixedwing_backside_main.cpp | 6 +- src/modules/mavlink/mavlink.c | 15 ++- src/modules/mavlink/orb_listener.c | 5 +- src/modules/mavlink_onboard/mavlink.c | 4 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 32 +++--- src/modules/uORB/topics/vehicle_control_mode.h | 3 - src/modules/uORB/topics/vehicle_status.h | 37 ++----- 16 files changed, 149 insertions(+), 164 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index e83a87aa9..9bb020a6b 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -536,7 +536,7 @@ BlinkM::led() /* looking for lipo cells that are connected */ printf(" checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf(" cells found:%d\n", num_of_cells); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea732eccd..827b0bb00 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -77,7 +77,7 @@ #include #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic int _t_actuator_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status + int _t_vehicle_control_mode; ///< vehicle control mode topic int _t_param; ///< parameter update topic /* advertised topics */ @@ -362,7 +362,7 @@ PX4IO::PX4IO() : _alarms(0), _t_actuators(-1), _t_actuator_armed(-1), - _t_vstatus(-1), + _t_vehicle_control_mode(-1), _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), @@ -647,8 +647,8 @@ PX4IO::task_main() _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ @@ -659,7 +659,7 @@ PX4IO::task_main() fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; - fds[2].fd = _t_vstatus; + fds[2].fd = _t_vehicle_control_mode; fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; @@ -834,10 +834,10 @@ int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state - vehicle_status_s vstatus; ///< overall system state + vehicle_control_mode_s control_mode; ///< vehicle_control_mode orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; @@ -853,7 +853,7 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } - if (vstatus.flag_external_manual_override_ok) { + if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d8b40ac3b..d1b941ffe 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include @@ -214,8 +214,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -228,8 +228,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode*/ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -281,9 +281,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39..2a06f1628 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include @@ -545,8 +545,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -559,8 +559,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -610,9 +610,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 144fda79a..c4dc495f6 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -171,7 +171,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -236,7 +236,7 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -248,7 +248,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -256,13 +256,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -275,14 +275,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -295,7 +295,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -346,7 +346,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -365,7 +365,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -426,7 +426,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -445,7 +445,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -580,7 +580,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = true; /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ // current_status.flag_vector_flight_mode_ok = false; @@ -637,32 +637,24 @@ int commander_thread_main(int argc, char *argv[]) pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); /* Start monitoring loop */ - uint16_t counter = 0; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; /* To remember when last notification was sent */ uint64_t last_print_time = 0; float voltage_previous = 0.0f; + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + uint64_t last_idle_time = 0; uint64_t start_time = 0; - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - bool state_changed = true; bool param_init_forced = true; @@ -764,10 +756,10 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || current_status.system_type == VEHICLE_TYPE_HEXAROTOR || current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; + control_mode.flag_external_manual_override_ok = false; } else { - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; } /* check and update system / component ID */ @@ -809,7 +801,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -848,16 +840,20 @@ int commander_thread_main(int argc, char *argv[]) orb_check(battery_sub, &new_data); if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; } /* update subsystem */ @@ -897,14 +893,15 @@ int commander_thread_main(int argc, char *argv[]) if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* XXX if armed */ - if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { + if (armed.armed) { /* armed, solid */ led_on(LED_AMBER); - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); } @@ -926,15 +923,16 @@ int commander_thread_main(int argc, char *argv[]) led_off(LED_BLUE); } - /* toggle GPS led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_BLUE); - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle arming (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - } + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } } @@ -948,35 +946,29 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); } low_voltage_counter++; } /* Critical, this is rather an emergency, change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - // XXX implement this - // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); + tune_critical_bat(); + // XXX implement state change here } critical_voltage_counter++; @@ -1585,7 +1577,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; + voltage_previous = current_status.battery_voltage; /* play tone according to evaluation result */ @@ -1595,10 +1587,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = true; /* Trigger audio event for low battery */ - } else if (bat_remain < 0.1f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (bat_remain < 0.2f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0b241d108..792ead8f3 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -510,7 +510,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /** * Transition from one hil state to another */ -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -530,7 +530,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = false; + current_control_mode->flag_system_hil_enabled = false; mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); valid_transition = true; } @@ -541,7 +541,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = true; + current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -558,8 +558,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status current_status->counter++; current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e591d2fef..75fdd224c 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -54,6 +54,6 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 9435959e9..0cfcfd51d 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" +#if 0 namespace control { @@ -277,11 +278,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { @@ -343,11 +345,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } #warning fix this // } @@ -382,3 +385,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() } // namespace control +#endif \ No newline at end of file diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526e..677a86771 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -150,12 +150,14 @@ int control_demo_thread_main(int argc, char *argv[]) using namespace control; - fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); +#warning fix this +// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); thread_running = true; while (!thread_should_exit) { - autopilot.update(); +#warning fix this +// autopilot.update(); } warnx("exiting."); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a2758a45c..5f683c443 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -191,7 +191,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* HIL */ - if (v_status.flag_hil_enabled) { + if (v_status.hil_state == HIL_STATE_ON) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -234,11 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_calibration) { + if (v_status.preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; @@ -677,7 +677,10 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, @@ -685,8 +688,8 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 57a5d5dd5..d088d421e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -272,7 +272,10 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 9ed2c6c12..c5dbd00dd 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -449,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b20d38b5e..ab3983019 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -945,8 +945,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ - log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; - log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; + log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; LOGBUFFER_WRITE_AND_COUNT(STAT); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index acc0c2717..5e334638f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -165,7 +165,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -352,9 +352,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -411,7 +411,7 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), @@ -941,21 +941,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !_hil_enabled) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1348,12 +1348,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* * do advertisements @@ -1406,7 +1406,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 02bf5568a..8481a624f 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -66,9 +66,6 @@ struct vehicle_control_mode_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_system_emergency; - bool flag_preflight_calibration; - // XXX needs yet to be set by state machine helper bool flag_system_hil_enabled; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2bcd57f4b..ec08a6c13 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -177,14 +177,11 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - /* system flags - these represent the state predicates */ - mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; - - bool condition_system_emergency; + bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; bool condition_system_returned_to_home; @@ -195,28 +192,6 @@ struct vehicle_status_s bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - // bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - //bool flag_armed; /**< true is motors / actuators are armed */ - //bool flag_safety_off; /**< true if safety is off */ - bool flag_system_emergency; - bool flag_preflight_calibration; - - // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // bool flag_auto_enabled; - // bool flag_control_rates_enabled; /**< true if rates are stabilized */ - // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - // bool flag_control_position_enabled; /**< true if position is controlled */ - - // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - // bool flag_preflight_accel_calibration; - // bool flag_preflight_airspeed_calibration; - bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ @@ -230,14 +205,20 @@ struct vehicle_status_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; + bool preflight_calibration; + + bool system_emergency; + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; + float load; - float voltage_battery; - float current_battery; + float battery_voltage; + float battery_current; float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ uint16_t drop_rate_comm; uint16_t errors_comm; -- cgit v1.2.3 From 68ab69de010adbe37b37558824ca013ecd0d569a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:47:51 +0200 Subject: moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results --- src/modules/commander/accelerometer_calibration.c | 453 ------ .../commander/accelerometer_calibration.cpp | 464 ++++++ src/modules/commander/airspeed_calibration.c | 111 -- src/modules/commander/airspeed_calibration.cpp | 113 ++ src/modules/commander/baro_calibration.c | 54 - src/modules/commander/baro_calibration.cpp | 54 + src/modules/commander/calibration_routines.c | 220 --- src/modules/commander/calibration_routines.cpp | 220 +++ src/modules/commander/commander.c | 1721 -------------------- src/modules/commander/commander.cpp | 1720 +++++++++++++++++++ src/modules/commander/commander.h | 54 - src/modules/commander/commander_helper.c | 218 --- src/modules/commander/commander_helper.cpp | 219 +++ src/modules/commander/commander_params.c | 54 + src/modules/commander/gyro_calibration.c | 231 --- src/modules/commander/gyro_calibration.cpp | 279 ++++ src/modules/commander/mag_calibration.c | 278 ---- src/modules/commander/mag_calibration.cpp | 280 ++++ src/modules/commander/module.mk | 21 +- src/modules/commander/rc_calibration.c | 83 - src/modules/commander/rc_calibration.cpp | 83 + src/modules/commander/state_machine_helper.c | 758 --------- src/modules/commander/state_machine_helper.cpp | 763 +++++++++ src/modules/systemlib/systemlib.h | 7 +- 24 files changed, 4264 insertions(+), 4194 deletions(-) delete mode 100644 src/modules/commander/accelerometer_calibration.c create mode 100644 src/modules/commander/accelerometer_calibration.cpp delete mode 100644 src/modules/commander/airspeed_calibration.c create mode 100644 src/modules/commander/airspeed_calibration.cpp delete mode 100644 src/modules/commander/baro_calibration.c create mode 100644 src/modules/commander/baro_calibration.cpp delete mode 100644 src/modules/commander/calibration_routines.c create mode 100644 src/modules/commander/calibration_routines.cpp delete mode 100644 src/modules/commander/commander.c create mode 100644 src/modules/commander/commander.cpp delete mode 100644 src/modules/commander/commander.h delete mode 100644 src/modules/commander/commander_helper.c create mode 100644 src/modules/commander/commander_helper.cpp create mode 100644 src/modules/commander/commander_params.c delete mode 100644 src/modules/commander/gyro_calibration.c create mode 100644 src/modules/commander/gyro_calibration.cpp delete mode 100644 src/modules/commander/mag_calibration.c create mode 100644 src/modules/commander/mag_calibration.cpp delete mode 100644 src/modules/commander/rc_calibration.c create mode 100644 src/modules/commander/rc_calibration.cpp delete mode 100644 src/modules/commander/state_machine_helper.c create mode 100644 src/modules/commander/state_machine_helper.cpp (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c deleted file mode 100644 index bc9d24956..000000000 --- a/src/modules/commander/accelerometer_calibration.c +++ /dev/null @@ -1,453 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file accelerometer_calibration.c - * - * Implementation of accelerometer calibration. - * - * Transform acceleration vector to true orientation, scale and offset - * - * ===== Model ===== - * accel_corr = accel_T * (accel_raw - accel_offs) - * - * accel_corr[3] - fully corrected acceleration vector in body frame - * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform - * accel_raw[3] - raw acceleration vector - * accel_offs[3] - acceleration offset vector - * - * ===== Calibration ===== - * - * Reference vectors - * accel_corr_ref[6][3] = [ g 0 0 ] // nose up - * | -g 0 0 | // nose down - * | 0 g 0 | // left side down - * | 0 -g 0 | // right side down - * | 0 0 g | // on back - * [ 0 0 -g ] // level - * accel_raw_ref[6][3] - * - * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 - * - * 6 reference vectors * 3 axes = 18 equations - * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants - * - * Find accel_offs - * - * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 - * - * Find accel_T - * - * 9 unknown constants - * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations - * - * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 - * - * Solve separate system for each row of accel_T: - * - * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 - * - * A * x = b - * - * x = [ accel_T[0][i] ] - * | accel_T[1][i] | - * [ accel_T[2][i] ] - * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough - * | accel_corr_ref[2][i] | - * [ accel_corr_ref[4][i] ] - * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 - * - * Matrix A is common for all three systems: - * A = [ a[0][0] a[0][1] a[0][2] ] - * | a[2][0] a[2][1] a[2][2] | - * [ a[4][0] a[4][1] a[4][2] ] - * - * x = A^-1 * b - * - * accel_T = A^-1 * g - * g = 9.80665 - * - * @author Anton Babushkin - */ - -#include "accelerometer_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include - - -#include -#include -#include -#include -#include -#include -#include - -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); -int detect_orientation(int mavlink_fd, int sub_sensor_combined); -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); -int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); - -void do_accel_calibration(int mavlink_fd) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - - /* measure and calculate offsets & scales */ - float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); - - if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); - } - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_positive(); - } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_negative(); - } - - /* exit accel calibration mode */ -} - -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { - const int samples_num = 2500; - float accel_ref[6][3]; - bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - while (true) { - bool done = true; - char str[80]; - int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); - for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); - done = false; - } - } - if (done) - break; - mavlink_log_info(mavlink_fd, str); - - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) - return ERROR; - - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); - read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); - data_collected[orient] = true; - tune_neutral(); - } - close(sensor_combined_sub); - - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); - return ERROR; - } - - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; - } - - return OK; -} - -/* - * Wait for vehicle become still and detect it's orientation. - * - * @return 0..5 according to orientation when vehicle is still and ready for measurements, - * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 - */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { - struct sensor_combined_s sensor; - /* exponential moving average of accel */ - float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; - /* max-hold dispersion of accel */ - float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - /* EMA time constant in seconds*/ - float ema_len = 0.2f; - /* set "still" threshold to 0.1 m/s^2 */ - float still_thr2 = pow(0.1f, 2); - /* set accel error threshold to 5m/s^2 */ - float accel_err_thr = 5.0f; - /* still time required in us */ - int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - hrt_abstime t_start = hrt_absolute_time(); - /* set timeout to 30s */ - hrt_abstime timeout = 30000000; - hrt_abstime t_timeout = t_start + timeout; - hrt_abstime t = t_start; - hrt_abstime t_prev = t_start; - hrt_abstime t_still = 0; - - unsigned poll_errcount = 0; - - while (true) { - /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); - t = hrt_absolute_time(); - float dt = (t - t_prev) / 1000000.0f; - t_prev = t; - float w = dt / ema_len; - for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; - d = d * d; - accel_disp[i] = accel_disp[i] * (1.0f - w); - if (d > accel_disp[i]) - accel_disp[i] = d; - } - /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { - /* is still now */ - if (t_still == 0) { - /* first time */ - mavlink_log_info(mavlink_fd, "still..."); - t_still = t; - t_timeout = t + timeout; - } else { - /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { - /* vehicle is still, exit from the loop to detection of its orientation */ - break; - } - } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { - /* not still, reset still start time */ - if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); - t_still = 0; - } - } - } else if (poll_ret == 0) { - poll_errcount++; - } - if (t > t_timeout) { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); - return -1; - } - } - - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) - return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) - return 5; // [ 0, 0, -g ] - - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); - - return -2; // Can't detect orientation -} - -/* - * Read specified number of accelerometer samples, calculate average and dispersion. - */ -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; - int count = 0; - float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; - - while (count < samples_num) { - int poll_ret = poll(fds, 1, 1000); - if (poll_ret == 1) { - struct sensor_combined_s sensor; - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) - accel_sum[i] += sensor.accelerometer_m_s2[i]; - count++; - } else { - return ERROR; - } - } - - for (int i = 0; i < 3; i++) { - accel_avg[i] = accel_sum[i] / count; - } - - return OK; -} - -int mat_invert3(float src[3][3], float dst[3][3]) { - float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) - return ERROR; // Singular matrix - - dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; - dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; - dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; - dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; - dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; - dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; - dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; - dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; - dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - - return OK; -} - -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { - /* calculate offsets */ - for (int i = 0; i < 3; i++) { - accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; - } - - /* fill matrix A for linear equations system*/ - float mat_A[3][3]; - memset(mat_A, 0, sizeof(mat_A)); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - float a = accel_ref[i * 2][j] - accel_offs[j]; - mat_A[i][j] = a; - } - } - - /* calculate inverse matrix for A */ - float mat_A_inv[3][3]; - if (mat_invert3(mat_A, mat_A_inv) != OK) - return ERROR; - - /* copy results to accel_T */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - /* simplify matrices mult because b has only one non-zero element == g at index i */ - accel_T[j][i] = mat_A_inv[j][i] * g; - } - } - - return OK; -} diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp new file mode 100644 index 000000000..df92d51d2 --- /dev/null +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -0,0 +1,464 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file accelerometer_calibration.cpp + * + * Implementation of accelerometer calibration. + * + * Transform acceleration vector to true orientation, scale and offset + * + * ===== Model ===== + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in body frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - acceleration offset vector + * + * ===== Calibration ===== + * + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // nose up + * | -g 0 0 | // nose down + * | 0 g 0 | // left side down + * | 0 -g 0 | // right side down + * | 0 0 g | // on back + * [ 0 0 -g ] // level + * accel_raw_ref[6][3] + * + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 + * + * 6 reference vectors * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * + * A * x = b + * + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] + * + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 + * + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] + * + * x = A^-1 * b + * + * accel_T = A^-1 * g + * g = 9.80665 + * + * @author Anton Babushkin + */ + +#include "accelerometer_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); + +void do_accel_calibration(int mavlink_fd) { + /* announce change */ + mavlink_log_info(mavlink_fd, "accel calibration started"); + + /* measure and calculate offsets & scales */ + float accel_offs[3]; + float accel_scale[3]; + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + + if (res == OK) { + /* measurements complete successfully, set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + } + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offs[0], + accel_scale[0], + accel_offs[1], + accel_scale[1], + accel_offs[2], + accel_scale[2], + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + mavlink_log_info(mavlink_fd, "accel calibration done"); + tune_positive(); + } else { + /* measurements error */ + mavlink_log_info(mavlink_fd, "accel calibration aborted"); + tune_negative(); + } + + /* exit accel calibration mode */ +} + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { + const int samples_num = 2500; + float accel_ref[6][3]; + bool data_collected[6] = { false, false, false, false, false, false }; + const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + + /* reset existing calibration */ + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); + close(fd); + + if (OK != ioctl_res) { + warn("ERROR: failed to set scale / offsets for accel"); + return ERROR; + } + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + while (true) { + bool done = true; + char str[80]; + int str_ptr; + str_ptr = sprintf(str, "keep vehicle still:"); + for (int i = 0; i < 6; i++) { + if (!data_collected[i]) { + str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); + done = false; + } + } + if (done) + break; + mavlink_log_info(mavlink_fd, str); + + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) + return ERROR; + + sprintf(str, "meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_neutral(); + } + close(sensor_combined_sub); + + /* calculate offsets and rotation+scale matrix */ + float accel_T[3][3]; + int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + return ERROR; + } + + /* convert accel transform matrix to scales, + * rotation part of transform matrix is not used by now + */ + for (int i = 0; i < 3; i++) { + accel_scale[i] = accel_T[i][i]; + } + + return OK; +} + +/* + * Wait for vehicle become still and detect it's orientation. + * + * @return 0..5 according to orientation when vehicle is still and ready for measurements, + * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 + */ +int detect_orientation(int mavlink_fd, int sub_sensor_combined) { + struct sensor_combined_s sensor; + /* exponential moving average of accel */ + float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; + /* max-hold dispersion of accel */ + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; + /* EMA time constant in seconds*/ + float ema_len = 0.2f; + /* set "still" threshold to 0.1 m/s^2 */ + float still_thr2 = pow(0.1f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; + /* still time required in us */ + int64_t still_time = 2000000; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + hrt_abstime t_start = hrt_absolute_time(); + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + for (int i = 0; i < 3; i++) { + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; + float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) + accel_disp[i] = d; + } + /* still detector with hysteresis */ + if ( accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2 ) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_log_info(mavlink_fd, "still..."); + t_still = t; + t_timeout = t + timeout; + } else { + /* still since t_still */ + if ((int64_t) t - (int64_t) t_still > still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + } else if ( accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_log_info(mavlink_fd, "moving..."); + t_still = 0; + } + } + } else if (poll_ret == 0) { + poll_errcount++; + } + if (t > t_timeout) { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); + return -1; + } + } + + if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 0; // [ g, 0, 0 ] + if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 1; // [ -g, 0, 0 ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 2; // [ 0, g, 0 ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 3; // [ 0, -g, 0 ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + return 4; // [ 0, 0, g ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + return 5; // [ 0, 0, -g ] + + mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + + return -2; // Can't detect orientation +} + +/* + * Read specified number of accelerometer samples, calculate average and dispersion. + */ +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; + int count = 0; + float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + + while (count < samples_num) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) + accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { + return ERROR; + } + } + + for (int i = 0; i < 3; i++) { + accel_avg[i] = accel_sum[i] / count; + } + + return OK; +} + +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0f) + return ERROR; // Singular matrix + + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + + return OK; +} + +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { + /* calculate offsets */ + for (int i = 0; i < 3; i++) { + accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; + } + + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = accel_ref[i * 2][j] - accel_offs[j]; + mat_A[i][j] = a; + } + } + + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) + return ERROR; + + /* copy results to accel_T */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + + return OK; +} diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c deleted file mode 100644 index feaf11aee..000000000 --- a/src/modules/commander/airspeed_calibration.c +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file airspeed_calibration.c - * Airspeed sensor calibration routine - */ - -#include "airspeed_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -void do_airspeed_calibration(int mavlink_fd) -{ - /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_positive(); - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - close(diff_pres_sub); -} diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp new file mode 100644 index 000000000..df08292e3 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed_calibration.cpp + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c deleted file mode 100644 index a70594794..000000000 --- a/src/modules/commander/baro_calibration.c +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file baro_calibration.c - * Barometer calibration routine - */ - -#include "baro_calibration.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -void do_baro_calibration(int mavlink_fd) -{ - // TODO implement this - return; -} diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp new file mode 100644 index 000000000..d7515b3d9 --- /dev/null +++ b/src/modules/commander/baro_calibration.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file baro_calibration.cpp + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +void do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return; +} diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c deleted file mode 100644 index 799cd4269..000000000 --- a/src/modules/commander/calibration_routines.c +++ /dev/null @@ -1,220 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file calibration_routines.c - * Calibration routines implementations. - * - * @author Lorenz Meier - */ - -#include - -#include "calibration_routines.h" - - -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) -{ - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A * A; - float B2 = B * B; - float C2 = C * C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * (QS - Rsq + QB + F0); - float aA, aB, aC, nA, nB, nC, dA, dB, dC; - - //Iterate N times, ignore stop condition. - int n = 0; - - while (n < max_iterations) { - n++; - - //Compute denominator: - aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); - aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); - aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; - - //Compute next iteration - nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); - nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); - nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - - if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A * A; - B2 = B * B; - C2 = C * C; - QS = A2 + B2 + C2; - QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * (QS - Rsq + QB + F0); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; -} - diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp new file mode 100644 index 000000000..be38ea104 --- /dev/null +++ b/src/modules/commander/calibration_routines.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file calibration_routines.cpp + * Calibration routines implementations. + * + * @author Lorenz Meier + */ + +#include + +#include "calibration_routines.h" + + +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) +{ + + float x_sumplain = 0.0f; + float x_sumsq = 0.0f; + float x_sumcube = 0.0f; + + float y_sumplain = 0.0f; + float y_sumsq = 0.0f; + float y_sumcube = 0.0f; + + float z_sumplain = 0.0f; + float z_sumsq = 0.0f; + float z_sumcube = 0.0f; + + float xy_sum = 0.0f; + float xz_sum = 0.0f; + float yz_sum = 0.0f; + + float x2y_sum = 0.0f; + float x2z_sum = 0.0f; + float y2x_sum = 0.0f; + float y2z_sum = 0.0f; + float z2x_sum = 0.0f; + float z2y_sum = 0.0f; + + for (unsigned int i = 0; i < size; i++) { + + float x2 = x[i] * x[i]; + float y2 = y[i] * y[i]; + float z2 = z[i] * z[i]; + + x_sumplain += x[i]; + x_sumsq += x2; + x_sumcube += x2 * x[i]; + + y_sumplain += y[i]; + y_sumsq += y2; + y_sumcube += y2 * y[i]; + + z_sumplain += z[i]; + z_sumsq += z2; + z_sumcube += z2 * z[i]; + + xy_sum += x[i] * y[i]; + xz_sum += x[i] * z[i]; + yz_sum += y[i] * z[i]; + + x2y_sum += x2 * y[i]; + x2z_sum += x2 * z[i]; + + y2x_sum += y2 * x[i]; + y2z_sum += y2 * z[i]; + + z2x_sum += z2 * x[i]; + z2y_sum += z2 * y[i]; + } + + // + //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data + // + // P is a structure that has been computed with the data earlier. + // P.npoints is the number of elements; the length of X,Y,Z are identical. + // P's members are logically named. + // + // X[n] is the x component of point n + // Y[n] is the y component of point n + // Z[n] is the z component of point n + // + // A is the x coordiante of the sphere + // B is the y coordiante of the sphere + // C is the z coordiante of the sphere + // Rsq is the radius squared of the sphere. + // + //This method should converge; maybe 5-100 iterations or more. + // + float x_sum = x_sumplain / size; //sum( X[n] ) + float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) + float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) + float y_sum = y_sumplain / size; //sum( Y[n] ) + float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) + float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) + float z_sum = z_sumplain / size; //sum( Z[n] ) + float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) + float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) + + float XY = xy_sum / size; //sum( X[n] * Y[n] ) + float XZ = xz_sum / size; //sum( X[n] * Z[n] ) + float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) + float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) + float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) + float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) + float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) + float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) + float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) + + //Reduction of multiplications + float F0 = x_sum2 + y_sum2 + z_sum2; + float F1 = 0.5f * F0; + float F2 = -8.0f * (x_sum3 + Y2X + Z2X); + float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); + float F4 = -8.0f * (X2Z + Y2Z + z_sum3); + + //Set initial conditions: + float A = x_sum; + float B = y_sum; + float C = z_sum; + + //First iteration computation: + float A2 = A * A; + float B2 = B * B; + float C2 = C * C; + float QS = A2 + B2 + C2; + float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + + //Set initial conditions: + float Rsq = F0 + QB + QS; + + //First iteration computation: + float Q0 = 0.5f * (QS - Rsq); + float Q1 = F1 + Q0; + float Q2 = 8.0f * (QS - Rsq + QB + F0); + float aA, aB, aC, nA, nB, nC, dA, dB, dC; + + //Iterate N times, ignore stop condition. + int n = 0; + + while (n < max_iterations) { + n++; + + //Compute denominator: + aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); + aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); + aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); + aA = (aA == 0.0f) ? 1.0f : aA; + aB = (aB == 0.0f) ? 1.0f : aB; + aC = (aC == 0.0f) ? 1.0f : aC; + + //Compute next iteration + nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); + nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); + nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); + + //Check for stop condition + dA = (nA - A); + dB = (nB - B); + dC = (nC - C); + + if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } + + //Compute next iteration's values + A = nA; + B = nB; + C = nC; + A2 = A * A; + B2 = B * B; + C2 = C * C; + QS = A2 + B2 + C2; + QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + Rsq = F0 + QB + QS; + Q0 = 0.5f * (QS - Rsq); + Q1 = F1 + Q0; + Q2 = 8.0f * (QS - Rsq + QB + F0); + } + + *sphere_x = A; + *sphere_y = B; + *sphere_z = C; + *sphere_radius = sqrtf(Rsq); + + return 0; +} + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c deleted file mode 100644 index c4dc495f6..000000000 --- a/src/modules/commander/commander.c +++ /dev/null @@ -1,1721 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file commander.c - * Main system state machine implementation. - * - */ - -#include "commander.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "commander_helper.h" -#include "state_machine_helper.h" -#include "calibration_routines.h" -#include "accelerometer_calibration.h" -#include "gyro_calibration.h" -#include "mag_calibration.h" -#include "baro_calibration.h" -#include "rc_calibration.h" -#include "airspeed_calibration.h" - -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - - -extern struct system_load_s system_load; - -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - -/* Decouple update interval and hysteris counters, all depends on intervals */ -#define COMMANDER_MONITORING_INTERVAL 50000 -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ - -/* Mavlink file descriptors */ -static int mavlink_fd; - -/* flags */ -static bool commander_initialized = false; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -/* timout until lowlevel failsafe */ -static unsigned int failsafe_lowlevel_timeout_ms; - -/* tasks waiting for low prio thread */ -enum { - LOW_PRIO_TASK_NONE = 0, - LOW_PRIO_TASK_PARAM_SAVE, - LOW_PRIO_TASK_PARAM_LOAD, - LOW_PRIO_TASK_GYRO_CALIBRATION, - LOW_PRIO_TASK_MAG_CALIBRATION, - LOW_PRIO_TASK_ALTITUDE_CALIBRATION, - LOW_PRIO_TASK_RC_CALIBRATION, - LOW_PRIO_TASK_ACCEL_CALIBRATION, - LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task; - - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -__EXPORT int commander_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -void usage(const char *reason); - -/** - * React to commands that are sent e.g. from the mavlink module. - */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); - -/** - * Mainloop of commander. - */ -int commander_thread_main(int argc, char *argv[]); - -/** - * Loop that runs at a lower rate and priority for calibration and parameter tasks. - */ -void *commander_low_prio_loop(void *arg); - - -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* request to set different system mode */ - switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: - - /* request to activate HIL */ - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - - if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - /* request to disarm */ - } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - - break; - - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - - /* request for an autopilot reboot */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { - /* reboot is executed later, after positive tune is sent */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - tune_positive(); - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - tune_negative(); - } - } - } - break; - - // /* request to land */ - // case VEHICLE_CMD_NAV_LAND: - // { - // //TODO: add check if landing possible - // //TODO: add landing maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } } - // break; - // - // /* request to takeoff */ - // case VEHICLE_CMD_NAV_TAKEOFF: - // { - // //TODO: add check if takeoff possible - // //TODO: add takeoff maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } - // } - // break; - // - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param1)) == 0) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; - - default: - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - break; - } - - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED || - result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - - tune_negative(); - - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - - tune_positive(); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0) { - /* send acknowledge command */ - // XXX TODO - } - -} - -int commander_thread_main(int argc, char *argv[]) -{ - /* not yet initialized */ - commander_initialized = false; - bool home_position_set = false; - - bool battery_tune_played = false; - bool arm_tune_played = false; - - /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - - /* welcome user */ - warnx("[commander] starting"); - - /* pthread for slow low prio thread */ - pthread_t commander_low_prio_thread; - - /* initialize */ - if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); - } - - if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); - } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); - } - - /* Main state machine */ - struct vehicle_status_s current_status; - orb_advert_t status_pub; - /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); - - /* armed topic */ - struct actuator_armed_s armed; - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - orb_advert_t control_mode_pub; - - /* Initialize all flags to false */ - memset(&control_mode, 0, sizeof(control_mode)); - - current_status.navigation_state = NAVIGATION_STATE_INIT; - current_status.arming_state = ARMING_STATE_INIT; - current_status.hil_state = HIL_STATE_OFF; - - /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; - - /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; - - /* allow manual override initially */ - control_mode.flag_external_manual_override_ok = true; - - /* flag position info as bad, do not allow auto mode */ - // current_status.flag_vector_flight_mode_ok = false; - - /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - - /* set safety device detection flag */ - /* XXX do we need this? */ - //current_status.flag_safety_present = false; - - // XXX for now just set sensors as initialized - current_status.condition_system_sensors_initialized = true; - - // XXX just disable offboard control for now - control_mode.flag_control_offboard_enabled = false; - - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); - /* publish current state machine */ - - /* publish the new state */ - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - - - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - - /* home position */ - orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); - - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } - - // XXX needed? - mavlink_log_info(mavlink_fd, "system is running"); - - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); - - struct sched_param param; - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - - /* Start monitoring loop */ - unsigned counter = 0; - unsigned low_voltage_counter = 0; - unsigned critical_voltage_counter = 0; - unsigned stick_off_counter = 0; - unsigned stick_on_counter = 0; - - /* To remember when last notification was sent */ - uint64_t last_print_time = 0; - - float voltage_previous = 0.0f; - - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; - - uint64_t last_idle_time = 0; - - uint64_t start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; - - bool new_data = false; - - /* Subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; - memset(&safety, 0, sizeof(safety)); - - /* Subscribe to manual control data */ - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; - memset(&sp_man, 0, sizeof(sp_man)); - - /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); - - /* Subscribe to global position */ - int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_s global_position; - memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; - - /* Subscribe to local position data */ - int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; - - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - - /* Subscribe to GPS topic */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - - /* Subscribe to sensor topic */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - - /* Subscribe to differential pressure topic */ - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - - /* Subscribe to parameters changed topic */ - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); - - /* Subscribe to battery topic */ - int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; - - /* Subscribe to subsystem info topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - memset(&info, 0, sizeof(info)); - - /* now initialized */ - commander_initialized = true; - thread_running = true; - - start_time = hrt_absolute_time(); - - while (!thread_should_exit) { - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - /* update parameters */ - if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed getting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - control_mode.flag_external_manual_override_ok = false; - - } else { - control_mode.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } - } - - orb_check(sp_man_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } - - orb_check(sp_offboard_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); - } - - orb_check(sensor_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - } - - orb_check(diff_pres_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; - } - - orb_check(cmd_sub, &new_data); - - if (new_data) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); - } - - /* update safety topic */ - orb_check(safety_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(safety), safety_sub, &safety); - } - - /* update global position estimate */ - orb_check(global_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; - } - - /* update local position estimate */ - orb_check(local_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; - } - - /* set the condition to valid if there has recently been a local position estimate */ - if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - current_status.condition_local_position_valid = true; - } else { - current_status.condition_local_position_valid = false; - } - - /* update battery status */ - orb_check(battery_sub, &new_data); - if (new_data) { - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - current_status.battery_voltage = battery.voltage_v; - current_status.condition_battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - - } - - if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { - current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); - } else { - current_status.battery_voltage = 0.0f; - } - - /* update subsystem */ - orb_check(subsys_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - current_status.onboard_control_sensors_present |= info.subsystem_type; - - } else { - current_status.onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - current_status.onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - current_status.onboard_control_sensors_health |= info.subsystem_type; - - } else { - current_status.onboard_control_sensors_health &= ~info.subsystem_type; - } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - - /* XXX if armed */ - if (armed.armed) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { - /* ready to arm */ - led_toggle(LED_AMBER); - } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not ready to arm, something is wrong */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - - // /* toggle GPS led at 5 Hz in HIL mode */ - // if (current_status.flag_hil_enabled) { - // /* hil enabled */ - // led_toggle(LED_BLUE); - - // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - // /* toggle arming (red) at 5 Hz on low battery or error */ - // led_toggle(LED_AMBER); - // } - - } - - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } - - - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - tune_low_bat(); - } - - low_voltage_counter++; - } - - /* Critical, this is rather an emergency, change state machine */ - else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - tune_critical_bat(); - // XXX implement state change here - } - - critical_voltage_counter++; - - } else { - low_voltage_counter = 0; - critical_voltage_counter = 0; - } - - /* End battery voltage check */ - - /* If in INIT state, try to proceed to STANDBY state */ - if (current_status.arming_state == ARMING_STATE_INIT) { - // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - } else { - // XXX: Add emergency stuff if sensors are lost - } - - - /* - * Check for valid position information. - * - * If the system has a valid position source from an onboard - * position estimator, it is safe to operate it autonomously. - * The flag_vector_flight_mode_ok flag indicates that a minimum - * set of position measurements is available. - */ - - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.condition_global_position_valid; - bool local_pos_valid = current_status.condition_local_position_valid; - bool airspeed_valid = current_status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_global_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.condition_global_position_valid = false; - } - - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_local_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_airspeed_valid = true; - - } else { - current_status.condition_airspeed_valid = false; - } - - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - // if (current_status.condition_local_position_valid || - // current_status.condition_global_position_valid) { - // current_status.flag_vector_flight_mode_ok = true; - - // } else { - // current_status.flag_vector_flight_mode_ok = false; - // } - - /* consolidate state change, flag as changed if required */ - if (global_pos_valid != current_status.condition_global_position_valid || - local_pos_valid != current_status.condition_local_position_valid || - airspeed_valid != current_status.condition_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - // if (!current_status.flag_valid_launch_position && - // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it - - // // XXX implement storage and publication of RTL position - // current_status.flag_valid_launch_position = true; - // current_status.flag_auto_flight_mode_ok = true; - // state_changed = true; - // } - - orb_check(gps_sub, &new_data); - if (new_data) { - - - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - - /* check if gps fix is ok */ - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !armed.armed) { - warnx("setting home position"); - - /* copy position data to uORB home message, store it locally as well */ - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - home_position_set = true; - tune_positive(); - } - } - - /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - - warnx("mode sw not finite"); - /* no valid stick position, go to default */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, go to manual mode */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.mode_switch = MODE_SWITCH_AUTO; - - } else { - - /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_SEATBELT; - } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if land/RTL is requested - */ - if (!isfinite(sp_man.return_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.return_switch = RETURN_SWITCH_RETURN; - - } else { - /* center stick position, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - } - - /* check mission switch */ - if (!isfinite(sp_man.mission_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { - - /* top switch position */ - current_status.mission_switch = MISSION_SWITCH_MISSION; - - } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom switch position */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else { - - /* center switch position, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? - } - - /* Now it's time to handle the stick inputs */ - - switch (current_status.arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* just manual, XXX this might depend on the return switch */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - - /* Try seatbelt or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - - /* Try auto or fallback to seatbelt or even manual */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - /* Always accept manual mode */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - - /* SEATBELT_STANDBY (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_NONE) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* SEATBELT_DESCENT (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_RETURN) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_NONE) { - - /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - - /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_MISSION) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - - /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_RETURN - && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - break; - - // XXX we might be missing something that triggers a transition from RTL to LAND - - case ARMING_STATE_ARMED_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - } - - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } - stick_off_counter = 0; - - } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - stick_on_counter = 0; - tune_positive(); - - } else { - stick_on_counter++; - stick_off_counter = 0; - } - } - - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; - - } else { - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - - if (!current_status.rc_signal_cutting_off) { - printf("Reason: not rc_signal_cutting_off\n"); - } else { - printf("last print time: %llu\n", last_print_time); - } - - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; - state_changed = true; - } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } - } - } - - - - - /* End mode switch */ - - /* END RC state check */ - - - /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - - // /* decide about attitude control flag, enable in att/pos/vel */ - // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - // /* decide about rate control flag, enable it always XXX (for now) */ - // bool rates_ctrl_enabled = true; - - // /* set up control mode */ - // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - // state_changed = true; - // } - - // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - // current_status.flag_control_rates_enabled = rates_ctrl_enabled; - // state_changed = true; - // } - - // /* handle the case where offboard control signal was regained */ - // if (!current_status.offboard_control_signal_found_once) { - // current_status.offboard_control_signal_found_once = true; - // /* enable offboard control, disable manual input */ - // current_status.flag_control_manual_enabled = false; - // current_status.flag_control_offboard_enabled = true; - // state_changed = true; - // tune_positive(); - - // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - // } else { - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - // state_changed = true; - // tune_positive(); - // } - // } - - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; - - // XXX check if this is correct - /* arm / disarm on request */ - if (sp_offboard.armed && !armed.armed) { - - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - - } else if (!sp_offboard.armed && armed.armed) { - - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - } - - } else { - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_positive(); - - /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; - state_changed = true; - } - } - } - } - - - - - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - - - // XXX this is missing - /* If full run came back clean, transition to standby */ - // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - // current_status.flag_preflight_gyro_calibration == false && - // current_status.flag_preflight_mag_calibration == false && - // current_status.flag_preflight_accel_calibration == false) { - // /* All ok, no calibration going on, go to standby */ - // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - // } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - state_changed = false; - } - - - - /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.battery_voltage; - - - /* play tone according to evaluation result */ - /* check if we recently armed */ - if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - if (tune_arm() == OK) - arm_tune_played = true; - - /* Trigger audio event for low battery */ - } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { - if (tune_critical_bat() == OK) - battery_tune_played = true; - } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { - if (tune_low_bat() == OK) - battery_tune_played = true; - } else if(battery_tune_played) { - tune_stop(); - battery_tune_played = false; - } - - /* reset arm_tune_played when disarmed */ - if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { - arm_tune_played = false; - } - - - /* XXX use this voltage_previous */ - fflush(stdout); - counter++; - usleep(COMMANDER_MONITORING_INTERVAL); - } - - /* wait for threads to complete */ - pthread_join(commander_low_prio_thread, NULL); - - /* close fds */ - led_deinit(); - buzzer_deinit(); - close(sp_man_sub); - close(sp_offboard_sub); - close(local_position_sub); - close(global_position_sub); - close(gps_sub); - close(sensor_sub); - close(safety_sub); - close(cmd_sub); - close(subsys_sub); - close(diff_pres_sub); - close(param_changed_sub); - close(battery_sub); - - warnx("exiting"); - fflush(stdout); - - thread_running = false; - - return 0; -} - - -void *commander_low_prio_loop(void *arg) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander low prio", getpid()); - - while (!thread_should_exit) { - - switch (low_prio_task) { - - case LOW_PRIO_TASK_PARAM_LOAD: - - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); - } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_PARAM_SAVE: - - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); - } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_GYRO_CALIBRATION: - - do_gyro_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_MAG_CALIBRATION: - - do_mag_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - - // do_baro_calibration(mavlink_fd); - - case LOW_PRIO_TASK_RC_CALIBRATION: - - // do_rc_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_ACCEL_CALIBRATION: - - do_accel_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - - do_airspeed_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_NONE: - default: - /* slow down to 10Hz */ - usleep(100000); - break; - } - - } - - return 0; -} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp new file mode 100644 index 000000000..253b6295f --- /dev/null +++ b/src/modules/commander/commander.cpp @@ -0,0 +1,1720 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander.cpp + * Main system state machine implementation. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "commander_helper.h" +#include "state_machine_helper.h" +#include "calibration_routines.h" +#include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +extern struct system_load_s system_load; + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + +/* Mavlink file descriptors */ +static int mavlink_fd; + +/* flags */ +static bool commander_initialized = false; +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* timout until lowlevel failsafe */ +static unsigned int failsafe_lowlevel_timeout_ms; + +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + * + * @ingroup apps + */ +extern "C" __EXPORT int commander_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +void usage(const char *reason); + +/** + * React to commands that are sent e.g. from the mavlink module. + */ +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); + + +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: + + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + /* reboot is executed later, after positive tune is sent */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); + } + } + } + break; + + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + + } else if (((int)(cmd->param1)) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + default: + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + + tune_negative(); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + + tune_positive(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + bool battery_tune_played = false; + bool arm_tune_played = false; + + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + + /* welcome user */ + warnx("[commander] starting"); + + /* pthread for slow low prio thread */ + pthread_t commander_low_prio_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds"); + } + + if (buzzer_init() != OK) { + warnx("ERROR: Failed to initialize buzzer"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + } + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; + + /* neither manual nor offboard control commands have been received */ + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; + + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; + + /* allow manual override initially */ + control_mode.flag_external_manual_override_ok = true; + + /* flag position info as bad, do not allow auto mode */ + // current_status.flag_vector_flight_mode_ok = false; + + /* set battery warning flag */ + current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + + /* set safety device detection flag */ + /* XXX do we need this? */ + //current_status.flag_safety_present = false; + + // XXX for now just set sensors as initialized + current_status.condition_system_sensors_initialized = true; + + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + + /* advertise to ORB */ + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + + + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + // XXX needed? + mavlink_log_info(mavlink_fd, "system is running"); + + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + + /* Start monitoring loop */ + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; + + /* To remember when last notification was sent */ + uint64_t last_print_time = 0; + + float voltage_previous = 0.0f; + + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + bool new_data = false; + + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + /* Subscribe to global position */ + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + /* Subscribe to local position data */ + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + + /* Subscribe to GPS topic */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + /* Subscribe to sensor topic */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + /* Subscribe to differential pressure topic */ + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + uint64_t last_diff_pres_time = 0; + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* Subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + start_time = hrt_absolute_time(); + + while (!thread_should_exit) { + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!armed.armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed getting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + control_mode.flag_external_manual_override_ok = false; + + } else { + control_mode.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + + orb_check(sp_man_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(diff_pres_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + last_diff_pres_time = diff_pres.timestamp; + } + + orb_check(cmd_sub, &new_data); + + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + } + + /* update safety topic */ + orb_check(safety_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(safety), safety_sub, &safety); + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; + } + + /* update subsystem */ + orb_check(subsys_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + current_status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + current_status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + current_status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_health &= ~info.subsystem_type; + } + } + + /* Slow but important 8 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + + /* XXX if armed */ + if (armed.armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ + led_toggle(LED_AMBER); + } + + if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { + + /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ + if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + /* GPS lock */ + led_on(LED_BLUE); + + } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* no GPS lock, but GPS module is aquiring lock */ + led_toggle(LED_BLUE); + } + + } else { + /* no GPS info, don't light the blue led */ + led_off(LED_BLUE); + } + + + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } + + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + + + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, change state machine */ + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + tune_critical_bat(); + // XXX implement state change here + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + // XXX check for sensors + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + } else { + // XXX: Add emergency stuff if sensors are lost + } + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_airspeed_valid = true; + + } else { + current_status.condition_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; + + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } + + /* consolidate state change, flag as changed if required */ + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it + + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } + + orb_check(gps_sub, &new_data); + if (new_data) { + + + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; + + /* check if gps fix is ok */ + // XXX magic number + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_positive(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* Start RC state check */ + + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + + warnx("mode sw not finite"); + /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + + } else { + + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if land/RTL is requested + */ + if (!isfinite(sp_man.return_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.return_switch = RETURN_SWITCH_RETURN; + + } else { + /* center stick position, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else { + + /* center switch position, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { + + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { + mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); + tune_negative(); + } else { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); + } + + } else { + mavlink_log_critical(mavlink_fd, "DISARM not allowed"); + tune_negative(); + } + stick_off_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position and we're in manual --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + stick_on_counter = 0; + tune_positive(); + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + state_changed = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } + } + } + + + + + /* End mode switch */ + + /* END RC state check */ + + + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; + + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } + + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } + + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); + + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + // XXX check if this is correct + /* arm / disarm on request */ + if (sp_offboard.armed && !armed.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + + } else if (!sp_offboard.armed && armed.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + } + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + tune_positive(); + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + current_status.failsave_lowlevel = true; + state_changed = true; + } + } + } + } + + + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + + + // XXX this is missing + /* If full run came back clean, transition to standby */ + // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + // current_status.flag_preflight_gyro_calibration == false && + // current_status.flag_preflight_mag_calibration == false && + // current_status.flag_preflight_accel_calibration == false) { + // /* All ok, no calibration going on, go to standby */ + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + state_changed = false; + } + + + + /* Store old modes to detect and act on state transitions */ + voltage_previous = current_status.battery_voltage; + + + /* play tone according to evaluation result */ + /* check if we recently armed */ + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (tune_arm() == OK) + arm_tune_played = true; + + /* Trigger audio event for low battery */ + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + if (tune_critical_bat() == OK) + battery_tune_played = true; + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + if (tune_low_bat() == OK) + battery_tune_played = true; + } else if(battery_tune_played) { + tune_stop(); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + + /* XXX use this voltage_previous */ + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + pthread_join(commander_low_prio_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(local_position_sub); + close(global_position_sub); + close(gps_sub); + close(sensor_sub); + close(safety_sub); + close(cmd_sub); + close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); + + warnx("exiting"); + fflush(stdout); + + thread_running = false; + + return 0; +} + + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + + // do_baro_calibration(mavlink_fd); + + case LOW_PRIO_TASK_RC_CALIBRATION: + + // do_rc_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h deleted file mode 100644 index 6e57c0ba5..000000000 --- a/src/modules/commander/commander.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file commander.h - * Main system state machine definition. - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - */ - -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - - - -#endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c deleted file mode 100644 index 199f73e6c..000000000 --- a/src/modules/commander/commander_helper.c +++ /dev/null @@ -1,218 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file commander_helper.c - * Commander helper functions implementations - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "commander_helper.h" - -bool is_multirotor(const struct vehicle_status_s *current_status) -{ - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); -} - -bool is_rotary_wing(const struct vehicle_status_s *current_status) -{ - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); -} - -static int buzzer; - -int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return OK; -} - -void buzzer_deinit() -{ - close(buzzer); -} - -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, 2); -} - -void tune_positive() -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_neutral() -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void tune_negative() -{ - ioctl(buzzer, TONE_SET_ALARM, 5); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, 12); -} - -int tune_critical_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, 14); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, 13); -} - -void tune_stop() -{ - ioctl(buzzer, TONE_SET_ALARM, 0); -} - -static int leds; - -int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -void led_deinit() -{ - close(leds); -} - -int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} \ No newline at end of file diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp new file mode 100644 index 000000000..9427bd892 --- /dev/null +++ b/src/modules/commander/commander_helper.cpp @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_helper.cpp + * Commander helper functions implementations + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, 12); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 14); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, 0); +} + +static int leds; + +int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +void led_deinit() +{ + close(leds); +} + +int led_toggle(int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; +} \ No newline at end of file diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c new file mode 100644 index 000000000..6832fc5ce --- /dev/null +++ b/src/modules/commander/commander_params.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander_params.c + * + * Parameters defined by the sensors task. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + */ + +#include +#include + +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c deleted file mode 100644 index 865f74ab4..000000000 --- a/src/modules/commander/gyro_calibration.c +++ /dev/null @@ -1,231 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file gyro_calibration.c - * Gyroscope calibration routine - */ - -#include "gyro_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -void do_gyro_calibration(int mavlink_fd) -{ - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - - - - /*** --- SCALING --- ***/ - - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); - mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); - warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); - - unsigned rotations_count = 30; - float gyro_integral = 0.0f; - float baseline_integral = 0.0f; - - // XXX change to mag topic - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; - if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; - - - uint64_t last_time = hrt_absolute_time(); - uint64_t start_time = hrt_absolute_time(); - - while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { - - /* abort this loop if not rotated more than 180 degrees within 5 seconds */ - if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) - break; - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - - float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; - last_time = hrt_absolute_time(); - - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - // XXX this is just a proof of concept and needs world / body - // transformation and more - - //math::Vector2f magNav(raw.magnetometer_ga); - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2*M_PI_F; - if (mag < -M_PI_F) mag += 2*M_PI_F; - - float diff = mag - mag_last; - - if (diff > M_PI_F) diff -= 2*M_PI_F; - if (diff < -M_PI_F) diff += 2*M_PI_F; - - baseline_integral += diff; - mag_last = mag; - // Jump through some timing scale hoops to avoid - // operating near the 1e6/1e8 max sane resolution of float. - gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - - warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); - - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; - } - } - - float gyro_scale = baseline_integral / gyro_integral; - warnx("gyro scale: yaw (z): %6.4f", gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); - - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp new file mode 100644 index 000000000..9e6909db0 --- /dev/null +++ b/src/modules/commander/gyro_calibration.cpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.cpp + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + // XXX negative tune + return; + } + + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + return; + } + + + /*** --- SCALING --- ***/ + + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; + if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) + break; + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag > M_PI_F) mag -= 2*M_PI_F; + if (mag < -M_PI_F) mag += 2*M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2*M_PI_F; + if (diff < -M_PI_F) diff += 2*M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + + warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; + warnx("gyro scale: yaw (z): %6.4f", gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + + + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { + + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + gyro_scales[0], + gyro_offset[1], + gyro_scales[1], + gyro_offset[2], + gyro_scales[2], + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c deleted file mode 100644 index dbd31a7e7..000000000 --- a/src/modules/commander/mag_calibration.c +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mag_calibration.c - * Magnetometer calibration routine - */ - -#include "mag_calibration.h" -#include "commander_helper.h" -#include "calibration_routines.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -void do_mag_calibration(int mavlink_fd) -{ - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_neutral(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - close(sub_mag); -} \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp new file mode 100644 index 000000000..9a25103f8 --- /dev/null +++ b/src/modules/commander/mag_calibration.cpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag_calibration.cpp + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fef8e366b..91d75121e 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,14 +36,15 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - commander_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c \ - gyro_calibration.c \ - mag_calibration.c \ - baro_calibration.c \ - rc_calibration.c \ - airspeed_calibration.c +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c deleted file mode 100644 index a21d3f558..000000000 --- a/src/modules/commander/rc_calibration.c +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file rc_calibration.c - * Remote Control calibration routine - */ - -#include "rc_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include - - -void do_rc_calibration(int mavlink_fd) -{ - mavlink_log_info(mavlink_fd, "trim calibration starting"); - - /* XXX fix this */ - // if (current_status.rc_signal) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - tune_positive(); - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} \ No newline at end of file diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp new file mode 100644 index 000000000..0de411713 --- /dev/null +++ b/src/modules/commander/rc_calibration.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_calibration.cpp + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include + + +void do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.rc_signal) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c deleted file mode 100644 index 792ead8f3..000000000 --- a/src/modules/commander/state_machine_helper.c +++ /dev/null @@ -1,758 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.c - * State machine helper functions implementations - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" -#include "commander_helper.h" - - -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; - } else { - - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED) { - - /* sensors need to be initialized for STANDBY state */ - if (current_state->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: - - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { - - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - - } - break; - case ARMING_STATE_IN_AIR_RESTORE: - - /* XXX implement */ - break; - default: - break; - } - - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); - } - } - - return ret; -} - - - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; - - /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; - } else { - - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: - - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { - - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_MANUAL_STANDBY: - - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_MANUAL: - - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; - - case NAVIGATION_STATE_SEATBELT_STANDBY: - - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { - - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_SEATBELT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_SEATBELT_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - default: - break; - } - - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); - } - } - - - - return ret; -} - - -/** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case HIL_STATE_OFF: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; - - case HIL_STATE_ON: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; - - default: - warnx("Unknown hil state"); - break; - } - } - - if (valid_transition) { - current_status->hil_state = new_state; - - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - - current_control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); - - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); - } - - return ret; -} - - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} - diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp new file mode 100644 index 000000000..3cf60a99a --- /dev/null +++ b/src/modules/commander/state_machine_helper.cpp @@ -0,0 +1,763 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file state_machine_helper.cpp + * State machine helper functions implementations + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "state_machine_helper.h" +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { + + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_state->arming_state == ARMING_STATE_STANDBY) { + ret = OK; + armed->armed = false; + armed->ready_to_arm = false; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->condition_system_sensors_initialized) { + ret = OK; + armed->armed = false; + armed->ready_to_arm = true; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ + ret = OK; + armed->armed = true; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_ARMED) { + + /* XXX conditions for an error state? */ + ret = OK; + armed->armed = true; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + armed->armed = false; + armed->ready_to_arm = false; + } + break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + armed->armed = false; + armed->ready_to_arm = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + default: + break; + } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + armed->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + } + } + + return ret; +} + + + +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: + + /* transitions back to INIT are possible for calibration */ + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; + } + } + break; + + case NAVIGATION_STATE_MANUAL: + + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; + } + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); + } else if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state-> condition_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + default: + break; + } + + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + } + } + + + + return ret; +} + + +/** +* Transition from one hil state to another +*/ +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} + diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e52..356215b0e 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,12 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +//extern void up_systemreset(void) noreturn_function; +#include <../arch/common/up_internal.h> + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); -- cgit v1.2.3