diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index e83a87aa93..9bb020a6b4 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 ea732eccd1..827b0bb009 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 d8b40ac3b5..d1b941ffe2 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 3ca50fb39c..2a06f1628b 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 144fda79ac..c4dc495f6a 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 0b241d108d..792ead8f3c 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 e591d2fef9..75fdd224c4 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 9435959e9b..0cfcfd51d1 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 4803a526eb..677a867712 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 a2758a45c7..5f683c4437 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 57a5d5dd50..d088d421ed 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 9ed2c6c12e..c5dbd00dd9 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 b20d38b5ee..ab39830192 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 acc0c2717b..5e334638f4 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 02bf5568ab..8481a624fd 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 2bcd57f4bb..ec08a6c13a 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;