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

Introduced new actuator_safety topic

This commit is contained in:
Julian Oes 2013-06-14 13:53:26 +02:00
parent 236053a600
commit 90f5e30f2a
22 changed files with 424 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/drivers/ardrone_interface/ardrone_interface.c
View File

@ -55,6 +55,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <systemlib/systemlib.h>
@ -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 {

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

@ -116,6 +116,7 @@
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_gps_position.h>
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;

17
src/drivers/hil/hil.cpp
View File

@ -75,6 +75,7 @@
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@ -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();

19
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -74,6 +74,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_safety.h>
#include <systemlib/err.h>
@ -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 */

27
src/drivers/px4fmu/fmu.cpp
View File

@ -69,6 +69,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_safety.h>
#include <systemlib/err.h>
#include <systemlib/ppm_decode.h>
@ -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");

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

@ -75,6 +75,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
@ -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;

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

@ -73,8 +73,10 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
@ -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;
@ -1190,16 +1189,29 @@ int commander_thread_main(int argc, char *argv[])
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(&current_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), &current_status);
status_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
/* publish current state machine */
state_machine_publish(stat_pub, &current_status, mavlink_fd);
state_machine_publish(status_pub, &current_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, &current_status, &cmd);
handle_command(status_pub, &current_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, &param_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, &current_status, mavlink_fd);
// state_machine_emergency(status_pub, &current_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, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
arming_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) {
// first fallback to SEATBELT_STANDY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
// or fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) {
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) {
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
} else if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) {
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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, &current_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) {
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_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) ||
// 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) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) &&
(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, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
) {
arming_state_transition(status_pub, &current_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, &current_status, ARMING_STATE_ARMED, mavlink_fd);
arming_state_transition(status_pub, &current_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, &current_status, ARMING_STATE_ARMED, mavlink_fd);
arming_state_transition(status_pub, &current_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, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
arming_state_transition(status_pub, &current_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, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
// do_state_update(status_pub, &current_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, &current_status);
orb_publish(ORB_ID(vehicle_status), status_pub, &current_status);
state_changed = false;
}

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

@ -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);
}
}

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

@ -46,6 +46,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
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);

16
src/modules/gpio_led/gpio_led.c
View File

@ -51,6 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
@ -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 {

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

@ -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));

3
src/modules/mavlink/orb_topics.h
View File

@ -58,6 +58,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
@ -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;

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

@ -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);

3
src/modules/mavlink_onboard/orb_topics.h
View File

@ -55,6 +55,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
@ -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;

2
src/modules/mavlink_onboard/util.h
View File

@ -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);

21
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -58,6 +58,7 @@
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
@ -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 */

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

@ -60,6 +60,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@ -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();

4
src/modules/uORB/objects_common.cpp
View File

@ -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"

15
src/modules/uORB/topics/actuator_controls.h
View File

@ -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

65
src/modules/uORB/topics/actuator_safety.h Normal file
View File

@ -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 <stdint.h>
#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

96
src/modules/uORB/topics/vehicle_control_mode.h Normal file
View File

@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
* 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 <stdint.h>
#include <stdbool.h>
#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

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

@ -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;
};
/**