dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Changed location of lots of flags and conditions, needs testing and more work

This commit is contained in:
Julian Oes 2013-07-16 18:55:32 +02:00
parent 6e44a486c1
commit bcdedd9a35
16 changed files with 149 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/blinkm/blinkm.cpp
View File

@ -536,7 +536,7 @@ BlinkM::led()
/* looking for lipo cells that are connected */ /* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n"); printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { 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("<blinkm> cells found:%d\n", num_of_cells); printf("<blinkm> cells found:%d\n", num_of_cells);

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

@ -77,7 +77,7 @@
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h> #include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
@ -180,7 +180,7 @@ private:
/* subscribed topics */ /* subscribed topics */
int _t_actuators; ///< actuator controls topic int _t_actuators; ///< actuator controls topic
int _t_actuator_armed; ///< system armed control 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 int _t_param; ///< parameter update topic
/* advertised topics */ /* advertised topics */
@ -362,7 +362,7 @@ PX4IO::PX4IO() :
_alarms(0), _alarms(0),
_t_actuators(-1), _t_actuators(-1),
_t_actuator_armed(-1), _t_actuator_armed(-1),
_t_vstatus(-1), _t_vehicle_control_mode(-1),
_t_param(-1), _t_param(-1),
_to_input_rc(0), _to_input_rc(0),
_to_actuators_effective(0), _to_actuators_effective(0),
@ -647,8 +647,8 @@ PX4IO::task_main()
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */
_t_param = orb_subscribe(ORB_ID(parameter_update)); _t_param = orb_subscribe(ORB_ID(parameter_update));
orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ orb_set_interval(_t_param, 500); /* 2Hz update rate max. */
@ -659,7 +659,7 @@ PX4IO::task_main()
fds[0].events = POLLIN; fds[0].events = POLLIN;
fds[1].fd = _t_actuator_armed; fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN; fds[1].events = POLLIN;
fds[2].fd = _t_vstatus; fds[2].fd = _t_vehicle_control_mode;
fds[2].events = POLLIN; fds[2].events = POLLIN;
fds[3].fd = _t_param; fds[3].fd = _t_param;
fds[3].events = POLLIN; fds[3].events = POLLIN;
@ -834,10 +834,10 @@ int
PX4IO::io_set_arming_state() PX4IO::io_set_arming_state()
{ {
actuator_armed_s armed; ///< system armed 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(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 set = 0;
uint16_t clear = 0; uint16_t clear = 0;
@ -853,7 +853,7 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; 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; set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else { } else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;

14
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -57,7 +57,7 @@
#include <uORB/topics/debug_key_value.h> #include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -214,8 +214,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memset(&raw, 0, sizeof(raw)); memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att)); memset(&att, 0, sizeof(att));
struct vehicle_status_s state; struct vehicle_control_mode_s control_mode;
memset(&state, 0, sizeof(state)); memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0; uint64_t last_data = 0;
uint64_t last_measurement = 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 */ /* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update)); int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/ /* subscribe to control mode*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status)); int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */ /* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); 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 */ /* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) { } else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */ /* 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, fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n"); "[att ekf] WARNING: Not getting sensors - sensor app running?\n");
} }

14
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
View File

@ -35,7 +35,7 @@
#include <uORB/topics/debug_key_value.h> #include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -545,8 +545,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att)); memset(&att, 0, sizeof(att));
struct vehicle_status_s state; struct vehicle_control_mode_s control_mode;
memset(&state, 0, sizeof(state)); memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0; uint64_t last_data = 0;
uint64_t last_measurement = 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 */ /* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update)); int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/ /* subscribe to control mode */
int sub_state = orb_subscribe(ORB_ID(vehicle_status)); int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */ /* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); 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 */ /* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) { } else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */ /* 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, fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
} }

122
src/modules/commander/commander.c
View File

@ -171,7 +171,7 @@ void usage(const char *reason);
/** /**
* React to commands that are sent e.g. from the mavlink module. * 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. * Mainloop of commander.
@ -236,7 +236,7 @@ void usage(const char *reason)
exit(1); 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 */ /* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; 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 */ /* request to activate HIL */
if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; 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 ((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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
} }
} else { } 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; 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 */ /* request to arm */
if ((int)cmd->param1 == 1) { 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
} }
/* request to disarm */ /* request to disarm */
} else if ((int)cmd->param1 == 0) { } 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; 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 */ /* request for an autopilot reboot */
if ((int)cmd->param1 == 1) { 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 */ /* reboot is executed later, after positive tune is sent */
result = VEHICLE_CMD_RESULT_ACCEPTED; result = VEHICLE_CMD_RESULT_ACCEPTED;
tune_positive(); 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) { if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */ /* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */ /* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; 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) { if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */ /* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */ /* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; 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) { if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */ /* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */ /* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; 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) { if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */ /* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */ /* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; 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; current_status.offboard_control_signal_lost = true;
/* allow manual override initially */ /* 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 */ /* flag position info as bad, do not allow auto mode */
// current_status.flag_vector_flight_mode_ok = false; // 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); pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
/* Start monitoring loop */ /* Start monitoring loop */
uint16_t counter = 0; unsigned counter = 0;
unsigned low_voltage_counter = 0;
/* Initialize to 0.0V */ unsigned critical_voltage_counter = 0;
float battery_voltage = 0.0f; unsigned stick_off_counter = 0;
bool battery_voltage_valid = false; unsigned stick_on_counter = 0;
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;
/* To remember when last notification was sent */ /* To remember when last notification was sent */
uint64_t last_print_time = 0; uint64_t last_print_time = 0;
float voltage_previous = 0.0f; 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 last_idle_time = 0;
uint64_t start_time = 0; uint64_t start_time = 0;
/* XXX use this! */
//uint64_t failsave_ll_start_time = 0;
bool state_changed = true; bool state_changed = true;
bool param_init_forced = 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 || if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
current_status.system_type == VEHICLE_TYPE_HEXAROTOR || current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
current_status.flag_external_manual_override_ok = false; control_mode.flag_external_manual_override_ok = false;
} else { } else {
current_status.flag_external_manual_override_ok = true; control_mode.flag_external_manual_override_ok = true;
} }
/* check and update system / component ID */ /* 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); orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */ /* handle it */
handle_command(status_pub, &current_status, &cmd, armed_pub, &armed); handle_command(status_pub, &current_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed);
} }
/* update safety topic */ /* update safety topic */
@ -848,16 +840,20 @@ int commander_thread_main(int argc, char *argv[])
orb_check(battery_sub, &new_data); orb_check(battery_sub, &new_data);
if (new_data) { if (new_data) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery); orb_copy(ORB_ID(battery_status), battery_sub, &battery);
battery_voltage = battery.voltage_v; current_status.battery_voltage = battery.voltage_v;
battery_voltage_valid = true; current_status.condition_battery_voltage_valid = true;
/* /*
* Only update battery voltage estimate if system has * Only update battery voltage estimate if system has
* been running for two and a half seconds. * 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 */ /* update subsystem */
@ -897,14 +893,15 @@ int commander_thread_main(int argc, char *argv[])
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
/* XXX if armed */ /* XXX if armed */
if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || if (armed.armed) {
current_status.state_machine == SYSTEM_STATE_AUTO ||
current_status.state_machine == SYSTEM_STATE_MANUAL*/)) {
/* armed, solid */ /* armed, solid */
led_on(LED_AMBER); led_on(LED_AMBER);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) {
/* not armed */ /* 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); led_toggle(LED_AMBER);
} }
@ -926,15 +923,16 @@ int commander_thread_main(int argc, char *argv[])
led_off(LED_BLUE); 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 GPS led at 5 Hz in HIL mode */
/* toggle arming (red) at 5 Hz on low battery or error */ // if (current_status.flag_hil_enabled) {
led_toggle(LED_AMBER); // /* 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; 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 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) { if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true; low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
tune_low_bat();
} }
low_voltage_counter++; low_voltage_counter++;
} }
/* Critical, this is rather an emergency, change state machine */ /* 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) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true; critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
// XXX implement this tune_critical_bat();
// state_machine_emergency(status_pub, &current_status, mavlink_fd); // XXX implement state change here
} }
critical_voltage_counter++; 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 */ /* 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 */ /* play tone according to evaluation result */
@ -1595,10 +1587,10 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = true; arm_tune_played = true;
/* Trigger audio event for low battery */ /* 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) if (tune_critical_bat() == OK)
battery_tune_played = true; 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) if (tune_low_bat() == OK)
battery_tune_played = true; battery_tune_played = true;
} else if(battery_tune_played) { } else if(battery_tune_played) {

11
src/modules/commander/state_machine_helper.c
View File

@ -510,7 +510,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/** /**
* Transition from one hil state to another * 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; bool valid_transition = false;
int ret = ERROR; 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 if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) { || 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"); mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true; 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 if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) { || 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"); mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true; 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->counter++;
current_status->timestamp = hrt_absolute_time(); current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status); 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; ret = OK;
} else { } else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");

2
src/modules/commander/state_machine_helper.h
View File

@ -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 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_ */ #endif /* STATE_MACHINE_HELPER_H_ */

24
src/modules/controllib/fixedwing.cpp
View File

@ -39,6 +39,7 @@
#include "fixedwing.hpp" #include "fixedwing.hpp"
#if 0
namespace control namespace control
{ {
@ -277,11 +278,12 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice. // This is not a hack, but a design choice.
/* do not limit in HIL */ /* do not limit in HIL */
if (!_status.flag_hil_enabled) { #warning fix this
/* limit to value of manual throttle */ // if (!_status.flag_hil_enabled) {
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? // /* limit to value of manual throttle */
_actuators.control[CH_THR] : _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) { } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
@ -343,11 +345,12 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice. // This is not a hack, but a design choice.
/* do not limit in HIL */ /* do not limit in HIL */
if (!_status.flag_hil_enabled) { #warning fix this
/* limit to value of manual throttle */ // if (!_status.flag_hil_enabled) {
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? // /* limit to value of manual throttle */
_actuators.control[CH_THR] : _manual.throttle; // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
} // _actuators.control[CH_THR] : _manual.throttle;
// }
#warning fix this #warning fix this
// } // }
@ -382,3 +385,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
} // namespace control } // namespace control
#endif

6
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
View File

@ -150,12 +150,14 @@ int control_demo_thread_main(int argc, char *argv[])
using namespace control; using namespace control;
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); #warning fix this
// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true; thread_running = true;
while (!thread_should_exit) { while (!thread_should_exit) {
autopilot.update(); #warning fix this
// autopilot.update();
} }
warnx("exiting."); warnx("exiting.");

15
src/modules/mavlink/mavlink.c
View File

@ -191,7 +191,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
**/ **/
/* HIL */ /* HIL */
if (v_status.flag_hil_enabled) { if (v_status.hil_state == HIL_STATE_ON) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; *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 */ /* set calibration state */
if (v_status.flag_preflight_calibration) { if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING; *mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.flag_system_emergency) { } else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_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); mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
/* switch HIL mode if required */ /* 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) */ /* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan, 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_enabled,
v_status.onboard_control_sensors_health, v_status.onboard_control_sensors_health,
v_status.load, v_status.load,
v_status.voltage_battery * 1000.0f, v_status.battery_voltage * 1000.0f,
v_status.current_battery * 1000.0f, v_status.battery_current * 1000.0f,
v_status.battery_remaining, v_status.battery_remaining,
v_status.drop_rate_comm, v_status.drop_rate_comm,
v_status.errors_comm, v_status.errors_comm,

5
src/modules/mavlink/orb_listener.c
View File

@ -272,7 +272,10 @@ l_vehicle_status(const struct listener *l)
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */ /* 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 */ /* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0; uint8_t mavlink_state = 0;

4
src/modules/mavlink_onboard/mavlink.c
View File

@ -449,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health, v_status.onboard_control_sensors_health,
v_status.load, v_status.load,
v_status.voltage_battery * 1000.0f, v_status.battery_voltage * 1000.0f,
v_status.current_battery * 1000.0f, v_status.battery_current * 1000.0f,
v_status.battery_remaining, v_status.battery_remaining,
v_status.drop_rate_comm, v_status.drop_rate_comm,
v_status.errors_comm, v_status.errors_comm,

4
src/modules/sdlog2/sdlog2.c
View File

@ -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_control_mode = 0;
log_msg.body.log_STAT.manual_sas_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.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_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.current_battery; 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_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
LOGBUFFER_WRITE_AND_COUNT(STAT); LOGBUFFER_WRITE_AND_COUNT(STAT);

32
src/modules/sensors/sensors.cpp
View File

@ -73,7 +73,7 @@
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
@ -165,7 +165,7 @@ private:
int _baro_sub; /**< raw baro data subscription */ int _baro_sub; /**< raw baro data subscription */
int _airspeed_sub; /**< airspeed subscription */ int _airspeed_sub; /**< airspeed subscription */
int _diff_pres_sub; /**< raw differential pressure 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 _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */ int _manual_control_sub; /**< notification of manual control updates */
@ -352,9 +352,9 @@ private:
void diff_pres_poll(struct sensor_combined_s &raw); 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. * Check for changes in parameters.
@ -411,7 +411,7 @@ Sensors::Sensors() :
_mag_sub(-1), _mag_sub(-1),
_rc_sub(-1), _rc_sub(-1),
_baro_sub(-1), _baro_sub(-1),
_vstatus_sub(-1), _vcontrol_mode_sub(-1),
_params_sub(-1), _params_sub(-1),
_manual_control_sub(-1), _manual_control_sub(-1),
@ -941,21 +941,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
} }
void void
Sensors::vehicle_status_poll() Sensors::vehicle_control_mode_poll()
{ {
struct vehicle_status_s vstatus; struct vehicle_control_mode_s vcontrol_mode;
bool vstatus_updated; bool vcontrol_mode_updated;
/* Check HIL state if vehicle status has changed */ /* Check HIL state if vehicle control mode has changed */
orb_check(_vstatus_sub, &vstatus_updated); 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 */ /* 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); //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; _hil_enabled = true;
_publishing = false; _publishing = false;
@ -1348,12 +1348,12 @@ Sensors::task_main()
_rc_sub = orb_subscribe(ORB_ID(input_rc)); _rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _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)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vcontrol_mode_sub, 200);
/* /*
* do advertisements * do advertisements
@ -1406,7 +1406,7 @@ Sensors::task_main()
perf_begin(_loop_perf); perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */ /* check vehicle status for changes to publication state */
vehicle_status_poll(); vehicle_control_mode_poll();
/* check parameters for updates */ /* check parameters for updates */
parameter_update_poll(); parameter_update_poll();

3
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -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_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 // XXX needs yet to be set by state machine helper
bool flag_system_hil_enabled; bool flag_system_hil_enabled;

37
src/modules/uORB/topics/vehicle_status.h
View File

@ -177,14 +177,11 @@ struct vehicle_status_s
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ 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 */ 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; mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch; return_switch_pos_t return_switch;
mission_switch_pos_t mission_switch; mission_switch_pos_t mission_switch;
bool condition_battery_voltage_valid;
bool condition_system_emergency;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_in_air_restore; /**< true if we can restore in mid air */
bool condition_system_sensors_initialized; bool condition_system_sensors_initialized;
bool condition_system_returned_to_home; bool condition_system_returned_to_home;
@ -195,28 +192,6 @@ struct vehicle_status_s
bool condition_local_position_valid; 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_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_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_lost; /**< true if RC reception is terminally lost */
bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ 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_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; bool failsave_highlevel;
bool preflight_calibration;
bool system_emergency;
/* see SYS_STATUS mavlink message for the following */ /* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_enabled;
uint32_t onboard_control_sensors_health; uint32_t onboard_control_sensors_health;
float load; float load;
float voltage_battery; float battery_voltage;
float current_battery; float battery_current;
float battery_remaining; float battery_remaining;
enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
uint16_t drop_rate_comm; uint16_t drop_rate_comm;
uint16_t errors_comm; uint16_t errors_comm;