forked from Archive/PX4-Autopilot
Renamed actuator_safety back to actuator_armed, compiling but untested
This commit is contained in:
parent
bf2ff98856
commit
1b38cf715d
|
@ -53,9 +53,10 @@
|
||||||
#include <sys/prctl.h>
|
#include <sys/prctl.h>
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/safety.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
|
|
||||||
|
@ -244,17 +245,15 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
||||||
int led_counter = 0;
|
int led_counter = 0;
|
||||||
|
|
||||||
/* declare and safely initialize all structs */
|
/* declare and safely initialize all structs */
|
||||||
struct vehicle_status_s state;
|
|
||||||
memset(&state, 0, sizeof(state));
|
|
||||||
struct actuator_controls_s actuator_controls;
|
struct actuator_controls_s actuator_controls;
|
||||||
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
safety.armed = false;
|
//XXX is this necessairy?
|
||||||
|
armed.armed = false;
|
||||||
|
|
||||||
/* subscribe to attitude, motor setpoints and system state */
|
/* subscribe to attitude, motor setpoints and system state */
|
||||||
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
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");
|
printf("[ardrone_interface] Motors initialized - ready.\n");
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
|
@ -325,12 +324,12 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* get a local copy of the actuator controls */
|
/* get a local copy of the actuator controls */
|
||||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||||
|
|
||||||
/* for now only spin if armed and immediately shut down
|
/* for now only spin if armed and immediately shut down
|
||||||
* if in failsafe
|
* if in failsafe
|
||||||
*/
|
*/
|
||||||
if (safety.armed && !safety.lockdown) {
|
if (armed.armed && !armed.lockdown) {
|
||||||
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
|
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -116,7 +116,8 @@
|
||||||
#include <poll.h>
|
#include <poll.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_gps_position.h>
|
#include <uORB/topics/vehicle_gps_position.h>
|
||||||
|
|
||||||
static const float MAX_CELL_VOLTAGE = 4.3f;
|
static const float MAX_CELL_VOLTAGE = 4.3f;
|
||||||
|
@ -376,8 +377,9 @@ BlinkM::led()
|
||||||
{
|
{
|
||||||
|
|
||||||
static int vehicle_status_sub_fd;
|
static int vehicle_status_sub_fd;
|
||||||
|
static int vehicle_control_mode_sub_fd;
|
||||||
static int vehicle_gps_position_sub_fd;
|
static int vehicle_gps_position_sub_fd;
|
||||||
static int actuator_safety_sub_fd;
|
static int actuator_armed_sub_fd;
|
||||||
|
|
||||||
static int num_of_cells = 0;
|
static int num_of_cells = 0;
|
||||||
static int detected_cells_runcount = 0;
|
static int detected_cells_runcount = 0;
|
||||||
|
@ -388,7 +390,8 @@ BlinkM::led()
|
||||||
static int led_interval = 1000;
|
static int led_interval = 1000;
|
||||||
|
|
||||||
static int no_data_vehicle_status = 0;
|
static int no_data_vehicle_status = 0;
|
||||||
static int no_data_actuator_safety = 0;
|
static int no_data_vehicle_control_mode = 0;
|
||||||
|
static int no_data_actuator_armed = 0;
|
||||||
static int no_data_vehicle_gps_position = 0;
|
static int no_data_vehicle_gps_position = 0;
|
||||||
|
|
||||||
static bool topic_initialized = false;
|
static bool topic_initialized = false;
|
||||||
|
@ -401,8 +404,11 @@ BlinkM::led()
|
||||||
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
|
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
orb_set_interval(vehicle_status_sub_fd, 1000);
|
orb_set_interval(vehicle_status_sub_fd, 1000);
|
||||||
|
|
||||||
actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety));
|
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
orb_set_interval(actuator_safety_sub_fd, 1000);
|
orb_set_interval(vehicle_control_mode_sub_fd, 1000);
|
||||||
|
|
||||||
|
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
|
orb_set_interval(actuator_armed_sub_fd, 1000);
|
||||||
|
|
||||||
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
|
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||||
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
|
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
|
||||||
|
@ -458,14 +464,16 @@ BlinkM::led()
|
||||||
if (led_thread_runcount == 15) {
|
if (led_thread_runcount == 15) {
|
||||||
/* obtained data for the first file descriptor */
|
/* obtained data for the first file descriptor */
|
||||||
struct vehicle_status_s vehicle_status_raw;
|
struct vehicle_status_s vehicle_status_raw;
|
||||||
struct actuator_safety_s actuator_safety;
|
struct vehicle_control_mode_s vehicle_control_mode;
|
||||||
|
struct actuator_armed_s actuator_armed;
|
||||||
struct vehicle_gps_position_s vehicle_gps_position_raw;
|
struct vehicle_gps_position_s vehicle_gps_position_raw;
|
||||||
|
|
||||||
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
|
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
|
||||||
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
|
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
|
||||||
|
|
||||||
bool new_data_vehicle_status;
|
bool new_data_vehicle_status;
|
||||||
bool new_data_actuator_safety;
|
bool new_data_vehicle_control_mode;
|
||||||
|
bool new_data_actuator_armed;
|
||||||
bool new_data_vehicle_gps_position;
|
bool new_data_vehicle_gps_position;
|
||||||
|
|
||||||
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
|
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
|
||||||
|
@ -479,15 +487,26 @@ BlinkM::led()
|
||||||
no_data_vehicle_status = 3;
|
no_data_vehicle_status = 3;
|
||||||
}
|
}
|
||||||
|
|
||||||
orb_check(actuator_safety_sub_fd, &new_data_actuator_safety);
|
orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
|
||||||
|
|
||||||
if (new_data_actuator_safety) {
|
if (new_data_vehicle_control_mode) {
|
||||||
orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety);
|
orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
|
||||||
no_data_actuator_safety = 0;
|
no_data_vehicle_control_mode = 0;
|
||||||
} else {
|
} else {
|
||||||
no_data_actuator_safety++;
|
no_data_vehicle_control_mode++;
|
||||||
if(no_data_vehicle_status >= 3)
|
if(no_data_vehicle_control_mode >= 3)
|
||||||
no_data_vehicle_status = 3;
|
no_data_vehicle_control_mode = 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
|
||||||
|
|
||||||
|
if (new_data_actuator_armed) {
|
||||||
|
orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
|
||||||
|
no_data_actuator_armed = 0;
|
||||||
|
} else {
|
||||||
|
no_data_actuator_armed++;
|
||||||
|
if(no_data_actuator_armed >= 3)
|
||||||
|
no_data_actuator_armed = 3;
|
||||||
}
|
}
|
||||||
|
|
||||||
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
|
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
|
||||||
|
@ -549,7 +568,7 @@ BlinkM::led()
|
||||||
} else {
|
} else {
|
||||||
/* no battery warnings here */
|
/* no battery warnings here */
|
||||||
|
|
||||||
if(actuator_safety.armed == false) {
|
if(actuator_armed.armed == false) {
|
||||||
/* system not armed */
|
/* system not armed */
|
||||||
led_color_1 = LED_RED;
|
led_color_1 = LED_RED;
|
||||||
led_color_2 = LED_RED;
|
led_color_2 = LED_RED;
|
||||||
|
@ -573,25 +592,21 @@ BlinkM::led()
|
||||||
led_color_8 = LED_OFF;
|
led_color_8 = LED_OFF;
|
||||||
led_blink = LED_BLINK;
|
led_blink = LED_BLINK;
|
||||||
|
|
||||||
/* handle 4th led - flightmode indicator */
|
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
|
||||||
#warning fix this
|
|
||||||
// switch((int)vehicle_status_raw.flight_mode) {
|
//XXX please check
|
||||||
// case VEHICLE_FLIGHT_MODE_MANUAL:
|
if (vehicle_control_mode.flag_control_position_enabled)
|
||||||
// led_color_4 = LED_AMBER;
|
led_color_4 = LED_GREEN;
|
||||||
// break;
|
else if (vehicle_control_mode.flag_control_velocity_enabled)
|
||||||
//
|
led_color_4 = LED_BLUE;
|
||||||
// case VEHICLE_FLIGHT_MODE_STAB:
|
else if (vehicle_control_mode.flag_control_attitude_enabled)
|
||||||
// led_color_4 = LED_YELLOW;
|
led_color_4 = LED_YELLOW;
|
||||||
// break;
|
else if (vehicle_control_mode.flag_control_manual_enabled)
|
||||||
//
|
led_color_4 = LED_AMBER;
|
||||||
// case VEHICLE_FLIGHT_MODE_HOLD:
|
else
|
||||||
// led_color_4 = LED_BLUE;
|
led_color_4 = LED_OFF;
|
||||||
// break;
|
|
||||||
//
|
}
|
||||||
// case VEHICLE_FLIGHT_MODE_AUTO:
|
|
||||||
// led_color_4 = LED_GREEN;
|
|
||||||
// break;
|
|
||||||
// }
|
|
||||||
|
|
||||||
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
|
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
|
||||||
/* handling used sat<61>s */
|
/* handling used sat<61>s */
|
||||||
|
|
|
@ -75,7 +75,7 @@
|
||||||
#include <systemlib/mixer/mixer.h>
|
#include <systemlib/mixer/mixer.h>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
@ -109,7 +109,7 @@ private:
|
||||||
int _current_update_rate;
|
int _current_update_rate;
|
||||||
int _task;
|
int _task;
|
||||||
int _t_actuators;
|
int _t_actuators;
|
||||||
int _t_safety;
|
int _t_armed;
|
||||||
orb_advert_t _t_outputs;
|
orb_advert_t _t_outputs;
|
||||||
unsigned _num_outputs;
|
unsigned _num_outputs;
|
||||||
bool _primary_pwm_device;
|
bool _primary_pwm_device;
|
||||||
|
@ -162,7 +162,7 @@ HIL::HIL() :
|
||||||
_current_update_rate(0),
|
_current_update_rate(0),
|
||||||
_task(-1),
|
_task(-1),
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
_t_safety(-1),
|
_t_armed(-1),
|
||||||
_t_outputs(0),
|
_t_outputs(0),
|
||||||
_num_outputs(0),
|
_num_outputs(0),
|
||||||
_primary_pwm_device(false),
|
_primary_pwm_device(false),
|
||||||
|
@ -322,8 +322,8 @@ HIL::task_main()
|
||||||
/* force a reset of the update rate */
|
/* force a reset of the update rate */
|
||||||
_current_update_rate = 0;
|
_current_update_rate = 0;
|
||||||
|
|
||||||
_t_safety = orb_subscribe(ORB_ID(actuator_safety));
|
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(_t_safety, 200); /* 5Hz update rate */
|
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
|
||||||
|
|
||||||
/* advertise the mixed control outputs */
|
/* advertise the mixed control outputs */
|
||||||
actuator_outputs_s outputs;
|
actuator_outputs_s outputs;
|
||||||
|
@ -335,7 +335,7 @@ HIL::task_main()
|
||||||
pollfd fds[2];
|
pollfd fds[2];
|
||||||
fds[0].fd = _t_actuators;
|
fds[0].fd = _t_actuators;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
fds[1].fd = _t_safety;
|
fds[1].fd = _t_armed;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
unsigned num_outputs;
|
unsigned num_outputs;
|
||||||
|
@ -427,15 +427,15 @@ HIL::task_main()
|
||||||
|
|
||||||
/* how about an arming update? */
|
/* how about an arming update? */
|
||||||
if (fds[1].revents & POLLIN) {
|
if (fds[1].revents & POLLIN) {
|
||||||
actuator_safety_s aa;
|
actuator_armed_s aa;
|
||||||
|
|
||||||
/* get new value */
|
/* get new value */
|
||||||
orb_copy(ORB_ID(actuator_safety), _t_safety, &aa);
|
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
::close(_t_actuators);
|
::close(_t_actuators);
|
||||||
::close(_t_safety);
|
::close(_t_armed);
|
||||||
|
|
||||||
/* make sure servos are off */
|
/* make sure servos are off */
|
||||||
// up_pwm_servo_deinit();
|
// up_pwm_servo_deinit();
|
||||||
|
|
|
@ -74,7 +74,7 @@
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_controls_effective.h>
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/esc_status.h>
|
#include <uORB/topics/esc_status.h>
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
@ -135,7 +135,7 @@ private:
|
||||||
int _current_update_rate;
|
int _current_update_rate;
|
||||||
int _task;
|
int _task;
|
||||||
int _t_actuators;
|
int _t_actuators;
|
||||||
int _t_actuator_safety;
|
int _t_actuator_armed;
|
||||||
unsigned int _motor;
|
unsigned int _motor;
|
||||||
int _px4mode;
|
int _px4mode;
|
||||||
int _frametype;
|
int _frametype;
|
||||||
|
@ -248,7 +248,7 @@ MK::MK(int bus) :
|
||||||
_update_rate(50),
|
_update_rate(50),
|
||||||
_task(-1),
|
_task(-1),
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
_t_actuator_safety(-1),
|
_t_actuator_armed(-1),
|
||||||
_t_outputs(0),
|
_t_outputs(0),
|
||||||
_t_actuators_effective(0),
|
_t_actuators_effective(0),
|
||||||
_t_esc_status(0),
|
_t_esc_status(0),
|
||||||
|
@ -513,8 +513,8 @@ MK::task_main()
|
||||||
/* force a reset of the update rate */
|
/* force a reset of the update rate */
|
||||||
_current_update_rate = 0;
|
_current_update_rate = 0;
|
||||||
|
|
||||||
_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
|
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
|
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
|
||||||
|
|
||||||
/* advertise the mixed control outputs */
|
/* advertise the mixed control outputs */
|
||||||
actuator_outputs_s outputs;
|
actuator_outputs_s outputs;
|
||||||
|
@ -540,7 +540,7 @@ MK::task_main()
|
||||||
pollfd fds[2];
|
pollfd fds[2];
|
||||||
fds[0].fd = _t_actuators;
|
fds[0].fd = _t_actuators;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
fds[1].fd = _t_actuator_safety;
|
fds[1].fd = _t_actuator_armed;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
log("starting");
|
log("starting");
|
||||||
|
@ -651,13 +651,13 @@ MK::task_main()
|
||||||
|
|
||||||
/* how about an arming update? */
|
/* how about an arming update? */
|
||||||
if (fds[1].revents & POLLIN) {
|
if (fds[1].revents & POLLIN) {
|
||||||
actuator_safety_s as;
|
actuator_armed_s aa;
|
||||||
|
|
||||||
/* get new value */
|
/* get new value */
|
||||||
orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
|
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
|
||||||
|
|
||||||
/* update PWM servo armed status if armed and not locked down */
|
/* update PWM servo armed status if armed and not locked down */
|
||||||
mk_servo_arm(as.armed && !as.lockdown);
|
mk_servo_arm(aa.armed && !aa.lockdown);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -700,7 +700,7 @@ MK::task_main()
|
||||||
//::close(_t_esc_status);
|
//::close(_t_esc_status);
|
||||||
::close(_t_actuators);
|
::close(_t_actuators);
|
||||||
::close(_t_actuators_effective);
|
::close(_t_actuators_effective);
|
||||||
::close(_t_actuator_safety);
|
::close(_t_actuator_armed);
|
||||||
|
|
||||||
|
|
||||||
/* make sure servos are off */
|
/* make sure servos are off */
|
||||||
|
|
|
@ -69,7 +69,7 @@
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_controls_effective.h>
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
#include <systemlib/ppm_decode.h>
|
#include <systemlib/ppm_decode.h>
|
||||||
|
@ -105,7 +105,7 @@ private:
|
||||||
unsigned _current_update_rate;
|
unsigned _current_update_rate;
|
||||||
int _task;
|
int _task;
|
||||||
int _t_actuators;
|
int _t_actuators;
|
||||||
int _t_actuator_safety;
|
int _t_actuator_armed;
|
||||||
orb_advert_t _t_outputs;
|
orb_advert_t _t_outputs;
|
||||||
orb_advert_t _t_actuators_effective;
|
orb_advert_t _t_actuators_effective;
|
||||||
unsigned _num_outputs;
|
unsigned _num_outputs;
|
||||||
|
@ -175,7 +175,7 @@ PX4FMU::PX4FMU() :
|
||||||
_current_update_rate(0),
|
_current_update_rate(0),
|
||||||
_task(-1),
|
_task(-1),
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
_t_actuator_safety(-1),
|
_t_actuator_armed(-1),
|
||||||
_t_outputs(0),
|
_t_outputs(0),
|
||||||
_t_actuators_effective(0),
|
_t_actuators_effective(0),
|
||||||
_num_outputs(0),
|
_num_outputs(0),
|
||||||
|
@ -377,8 +377,8 @@ PX4FMU::task_main()
|
||||||
/* force a reset of the update rate */
|
/* force a reset of the update rate */
|
||||||
_current_update_rate = 0;
|
_current_update_rate = 0;
|
||||||
|
|
||||||
_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
|
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
|
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
|
||||||
|
|
||||||
/* advertise the mixed control outputs */
|
/* advertise the mixed control outputs */
|
||||||
actuator_outputs_s outputs;
|
actuator_outputs_s outputs;
|
||||||
|
@ -397,7 +397,7 @@ PX4FMU::task_main()
|
||||||
pollfd fds[2];
|
pollfd fds[2];
|
||||||
fds[0].fd = _t_actuators;
|
fds[0].fd = _t_actuators;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
fds[1].fd = _t_actuator_safety;
|
fds[1].fd = _t_actuator_armed;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
|
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
|
||||||
|
@ -500,13 +500,13 @@ PX4FMU::task_main()
|
||||||
|
|
||||||
/* how about an arming update? */
|
/* how about an arming update? */
|
||||||
if (fds[1].revents & POLLIN) {
|
if (fds[1].revents & POLLIN) {
|
||||||
actuator_safety_s as;
|
actuator_armed_s aa;
|
||||||
|
|
||||||
/* get new value */
|
/* get new value */
|
||||||
orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
|
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
|
||||||
|
|
||||||
/* update PWM servo armed status if armed and not locked down */
|
/* update PWM servo armed status if armed and not locked down */
|
||||||
bool set_armed = as.armed && !as.lockdown;
|
bool set_armed = aa.armed && !aa.lockdown;
|
||||||
if (set_armed != _armed) {
|
if (set_armed != _armed) {
|
||||||
_armed = set_armed;
|
_armed = set_armed;
|
||||||
up_pwm_servo_arm(set_armed);
|
up_pwm_servo_arm(set_armed);
|
||||||
|
@ -536,7 +536,7 @@ PX4FMU::task_main()
|
||||||
|
|
||||||
::close(_t_actuators);
|
::close(_t_actuators);
|
||||||
::close(_t_actuators_effective);
|
::close(_t_actuators_effective);
|
||||||
::close(_t_actuator_safety);
|
::close(_t_actuator_armed);
|
||||||
|
|
||||||
/* make sure servos are off */
|
/* make sure servos are off */
|
||||||
up_pwm_servo_deinit();
|
up_pwm_servo_deinit();
|
||||||
|
@ -1022,12 +1022,12 @@ fake(int argc, char *argv[])
|
||||||
if (handle < 0)
|
if (handle < 0)
|
||||||
errx(1, "advertise failed");
|
errx(1, "advertise failed");
|
||||||
|
|
||||||
actuator_safety_s as;
|
actuator_armed_s aa;
|
||||||
|
|
||||||
as.armed = true;
|
aa.armed = true;
|
||||||
as.lockdown = false;
|
aa.lockdown = false;
|
||||||
|
|
||||||
handle = orb_advertise(ORB_ID(actuator_safety), &as);
|
handle = orb_advertise(ORB_ID(actuator_armed), &aa);
|
||||||
|
|
||||||
if (handle < 0)
|
if (handle < 0)
|
||||||
errx(1, "advertise failed 2");
|
errx(1, "advertise failed 2");
|
||||||
|
|
|
@ -75,7 +75,8 @@
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_controls_effective.h>
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
|
#include <uORB/topics/safety.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.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>
|
||||||
|
@ -178,7 +179,7 @@ private:
|
||||||
|
|
||||||
/* subscribed topics */
|
/* subscribed topics */
|
||||||
int _t_actuators; ///< actuator controls topic
|
int _t_actuators; ///< actuator controls topic
|
||||||
int _t_actuator_safety; ///< system armed control topic
|
int _t_actuator_armed; ///< system armed control topic
|
||||||
int _t_vstatus; ///< system / vehicle status
|
int _t_vstatus; ///< system / vehicle status
|
||||||
int _t_param; ///< parameter update topic
|
int _t_param; ///< parameter update topic
|
||||||
|
|
||||||
|
@ -360,7 +361,7 @@ PX4IO::PX4IO() :
|
||||||
_status(0),
|
_status(0),
|
||||||
_alarms(0),
|
_alarms(0),
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
_t_actuator_safety(-1),
|
_t_actuator_armed(-1),
|
||||||
_t_vstatus(-1),
|
_t_vstatus(-1),
|
||||||
_t_param(-1),
|
_t_param(-1),
|
||||||
_to_input_rc(0),
|
_to_input_rc(0),
|
||||||
|
@ -505,9 +506,9 @@ PX4IO::init()
|
||||||
* remains untouched (so manual override is still available).
|
* remains untouched (so manual override is still available).
|
||||||
*/
|
*/
|
||||||
|
|
||||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
/* fill with initial values, clear updated flag */
|
/* fill with initial values, clear updated flag */
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s safety;
|
||||||
uint64_t try_start_time = hrt_absolute_time();
|
uint64_t try_start_time = hrt_absolute_time();
|
||||||
bool updated = false;
|
bool updated = false;
|
||||||
|
|
||||||
|
@ -518,7 +519,7 @@ PX4IO::init()
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
/* got data, copy and exit loop */
|
/* got data, copy and exit loop */
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -559,7 +560,7 @@ PX4IO::init()
|
||||||
orb_check(safety_sub, &updated);
|
orb_check(safety_sub, &updated);
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* wait 50 ms */
|
/* wait 50 ms */
|
||||||
|
@ -643,8 +644,8 @@ PX4IO::task_main()
|
||||||
ORB_ID(actuator_controls_1));
|
ORB_ID(actuator_controls_1));
|
||||||
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
|
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
|
||||||
|
|
||||||
_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
|
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
|
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
|
||||||
|
|
||||||
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
|
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
|
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
|
||||||
|
@ -656,7 +657,7 @@ PX4IO::task_main()
|
||||||
pollfd fds[4];
|
pollfd fds[4];
|
||||||
fds[0].fd = _t_actuators;
|
fds[0].fd = _t_actuators;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
fds[1].fd = _t_actuator_safety;
|
fds[1].fd = _t_actuator_armed;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
fds[2].fd = _t_vstatus;
|
fds[2].fd = _t_vstatus;
|
||||||
fds[2].events = POLLIN;
|
fds[2].events = POLLIN;
|
||||||
|
@ -832,21 +833,21 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
|
||||||
int
|
int
|
||||||
PX4IO::io_set_arming_state()
|
PX4IO::io_set_arming_state()
|
||||||
{
|
{
|
||||||
actuator_safety_s safety; ///< system armed state
|
actuator_armed_s armed; ///< system armed state
|
||||||
vehicle_status_s vstatus; ///< overall system state
|
vehicle_status_s vstatus; ///< overall system state
|
||||||
|
|
||||||
orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
|
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
|
||||||
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
|
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
|
||||||
|
|
||||||
uint16_t set = 0;
|
uint16_t set = 0;
|
||||||
uint16_t clear = 0;
|
uint16_t clear = 0;
|
||||||
|
|
||||||
if (safety.armed && !safety.lockdown) {
|
if (armed.armed && !armed.lockdown) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||||
} else {
|
} else {
|
||||||
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||||
}
|
}
|
||||||
if (safety.ready_to_arm) {
|
if (armed.ready_to_arm) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||||
} else {
|
} else {
|
||||||
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||||
|
@ -1004,10 +1005,10 @@ PX4IO::io_handle_status(uint16_t status)
|
||||||
/**
|
/**
|
||||||
* Get and handle the safety status
|
* Get and handle the safety status
|
||||||
*/
|
*/
|
||||||
struct actuator_safety_s safety;
|
struct safety_s safety;
|
||||||
safety.timestamp = hrt_absolute_time();
|
safety.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
|
orb_copy(ORB_ID(safety), _to_safety, &safety);
|
||||||
|
|
||||||
if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
|
if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
|
||||||
safety.safety_off = true;
|
safety.safety_off = true;
|
||||||
|
@ -1019,9 +1020,9 @@ PX4IO::io_handle_status(uint16_t status)
|
||||||
|
|
||||||
/* lazily publish the safety status */
|
/* lazily publish the safety status */
|
||||||
if (_to_safety > 0) {
|
if (_to_safety > 0) {
|
||||||
orb_publish(ORB_ID(actuator_safety), _to_safety, &safety);
|
orb_publish(ORB_ID(safety), _to_safety, &safety);
|
||||||
} else {
|
} else {
|
||||||
_to_safety = orb_advertise(ORB_ID(actuator_safety), &safety);
|
_to_safety = orb_advertise(ORB_ID(safety), &safety);
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
|
|
|
@ -55,7 +55,7 @@
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
@ -159,7 +159,7 @@ flow_position_control_thread_main(int argc, char *argv[])
|
||||||
const float time_scale = powf(10.0f,-6.0f);
|
const float time_scale = powf(10.0f,-6.0f);
|
||||||
|
|
||||||
/* structures */
|
/* structures */
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
struct vehicle_control_mode_s control_mode;
|
struct vehicle_control_mode_s control_mode;
|
||||||
struct vehicle_attitude_s att;
|
struct vehicle_attitude_s att;
|
||||||
struct manual_control_setpoint_s manual;
|
struct manual_control_setpoint_s manual;
|
||||||
|
@ -171,7 +171,7 @@ flow_position_control_thread_main(int argc, char *argv[])
|
||||||
/* subscribe to attitude, motor setpoints and system state */
|
/* subscribe to attitude, motor setpoints and system state */
|
||||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||||
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||||
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
|
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
|
||||||
|
@ -261,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[])
|
||||||
perf_begin(mc_loop_perf);
|
perf_begin(mc_loop_perf);
|
||||||
|
|
||||||
/* get a local copy of the vehicle state */
|
/* get a local copy of the vehicle state */
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||||
/* get a local copy of manual setpoint */
|
/* get a local copy of manual setpoint */
|
||||||
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
|
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
|
||||||
/* get a local copy of attitude */
|
/* get a local copy of attitude */
|
||||||
|
@ -578,7 +578,7 @@ flow_position_control_thread_main(int argc, char *argv[])
|
||||||
close(parameter_update_sub);
|
close(parameter_update_sub);
|
||||||
close(vehicle_attitude_sub);
|
close(vehicle_attitude_sub);
|
||||||
close(vehicle_local_position_sub);
|
close(vehicle_local_position_sub);
|
||||||
close(safety_sub);
|
close(armed_sub);
|
||||||
close(control_mode_sub);
|
close(control_mode_sub);
|
||||||
close(manual_control_setpoint_sub);
|
close(manual_control_setpoint_sub);
|
||||||
close(speed_sp_pub);
|
close(speed_sp_pub);
|
||||||
|
|
|
@ -56,7 +56,7 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
|
@ -159,8 +159,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
||||||
static float sonar_lp = 0.0f;
|
static float sonar_lp = 0.0f;
|
||||||
|
|
||||||
/* subscribe to vehicle status, attitude, sensors and flow*/
|
/* subscribe to vehicle status, attitude, sensors and flow*/
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
memset(&safety, 0, sizeof(safety));
|
memset(&armed, 0, sizeof(armed));
|
||||||
struct vehicle_control_mode_s control_mode;
|
struct vehicle_control_mode_s control_mode;
|
||||||
memset(&control_mode, 0, sizeof(control_mode));
|
memset(&control_mode, 0, sizeof(control_mode));
|
||||||
struct vehicle_attitude_s att;
|
struct vehicle_attitude_s att;
|
||||||
|
@ -173,8 +173,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
||||||
/* subscribe to parameter changes */
|
/* subscribe to parameter changes */
|
||||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||||
|
|
||||||
/* subscribe to safety topic */
|
/* subscribe to armed topic */
|
||||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
|
|
||||||
/* subscribe to safety topic */
|
/* subscribe to safety topic */
|
||||||
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
|
@ -270,7 +270,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
||||||
/* got flow, updating attitude and status as well */
|
/* got flow, updating attitude and status as well */
|
||||||
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
|
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
|
||||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
|
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||||
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
||||||
|
|
||||||
/* vehicle state estimation */
|
/* vehicle state estimation */
|
||||||
|
@ -284,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (!vehicle_liftoff)
|
if (!vehicle_liftoff)
|
||||||
{
|
{
|
||||||
if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
|
if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
|
||||||
vehicle_liftoff = true;
|
vehicle_liftoff = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
|
if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
|
||||||
vehicle_liftoff = false;
|
vehicle_liftoff = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -356,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
/* filtering ground distance */
|
/* filtering ground distance */
|
||||||
if (!vehicle_liftoff || !safety.armed)
|
if (!vehicle_liftoff || !armed.armed)
|
||||||
{
|
{
|
||||||
/* not possible to fly */
|
/* not possible to fly */
|
||||||
sonar_valid = false;
|
sonar_valid = false;
|
||||||
|
@ -453,7 +453,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
close(vehicle_attitude_setpoint_sub);
|
close(vehicle_attitude_setpoint_sub);
|
||||||
close(vehicle_attitude_sub);
|
close(vehicle_attitude_sub);
|
||||||
close(safety_sub);
|
close(armed_sub);
|
||||||
close(control_mode_sub);
|
close(control_mode_sub);
|
||||||
close(parameter_update_sub);
|
close(parameter_update_sub);
|
||||||
close(optical_flow_sub);
|
close(optical_flow_sub);
|
||||||
|
|
|
@ -55,7 +55,7 @@
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
|
@ -156,7 +156,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
|
||||||
uint32_t counter = 0;
|
uint32_t counter = 0;
|
||||||
|
|
||||||
/* structures */
|
/* structures */
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
struct vehicle_control_mode_s control_mode;
|
struct vehicle_control_mode_s control_mode;
|
||||||
struct filtered_bottom_flow_s filtered_flow;
|
struct filtered_bottom_flow_s filtered_flow;
|
||||||
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
|
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
|
||||||
|
@ -166,7 +166,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
|
||||||
/* subscribe to attitude, motor setpoints and system state */
|
/* subscribe to attitude, motor setpoints and system state */
|
||||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||||
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
|
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
|
||||||
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
|
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
|
||||||
|
@ -229,8 +229,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
perf_begin(mc_loop_perf);
|
perf_begin(mc_loop_perf);
|
||||||
|
|
||||||
/* get a local copy of the safety topic */
|
/* get a local copy of the armed topic */
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||||
/* get a local copy of the control mode */
|
/* get a local copy of the control mode */
|
||||||
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
||||||
/* get a local copy of filtered bottom flow */
|
/* get a local copy of filtered bottom flow */
|
||||||
|
@ -355,7 +355,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
|
||||||
close(vehicle_attitude_sub);
|
close(vehicle_attitude_sub);
|
||||||
close(vehicle_bodyframe_speed_setpoint_sub);
|
close(vehicle_bodyframe_speed_setpoint_sub);
|
||||||
close(filtered_bottom_flow_sub);
|
close(filtered_bottom_flow_sub);
|
||||||
close(safety_sub);
|
close(armed_sub);
|
||||||
close(control_mode_sub);
|
close(control_mode_sub);
|
||||||
close(att_sp_pub);
|
close(att_sp_pub);
|
||||||
|
|
||||||
|
|
|
@ -76,9 +76,10 @@
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
#include <uORB/topics/subsystem_info.h>
|
#include <uORB/topics/subsystem_info.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <uORB/topics/differential_pressure.h>
|
#include <uORB/topics/differential_pressure.h>
|
||||||
|
#include <uORB/topics/safety.h>
|
||||||
#include <mavlink/mavlink_log.h>
|
#include <mavlink/mavlink_log.h>
|
||||||
|
|
||||||
#include <drivers/drv_led.h>
|
#include <drivers/drv_led.h>
|
||||||
|
@ -177,7 +178,7 @@ int led_off(int led);
|
||||||
void do_reboot(void);
|
void do_reboot(void);
|
||||||
|
|
||||||
|
|
||||||
void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety);
|
void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed);
|
||||||
|
|
||||||
// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
|
// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
|
||||||
|
|
||||||
|
@ -253,7 +254,7 @@ void do_reboot()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety)
|
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed)
|
||||||
{
|
{
|
||||||
/* result of the command */
|
/* result of the command */
|
||||||
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
|
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||||
|
@ -273,13 +274,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
|
if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
|
||||||
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
} else {
|
} else {
|
||||||
result = VEHICLE_CMD_RESULT_DENIED;
|
result = VEHICLE_CMD_RESULT_DENIED;
|
||||||
|
@ -292,14 +293,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
|
|
||||||
/* request to arm */
|
/* 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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
} else {
|
} else {
|
||||||
result = VEHICLE_CMD_RESULT_DENIED;
|
result = VEHICLE_CMD_RESULT_DENIED;
|
||||||
|
@ -312,7 +313,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
|
|
||||||
/* request for an autopilot reboot */
|
/* 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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) {
|
||||||
/* reboot is executed later, after positive tune is sent */
|
/* reboot is executed later, after positive tune is sent */
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
tune_positive();
|
tune_positive();
|
||||||
|
@ -361,7 +362,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
if(low_prio_task == LOW_PRIO_TASK_NONE) {
|
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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
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;
|
||||||
|
@ -380,7 +381,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
if(low_prio_task == LOW_PRIO_TASK_NONE) {
|
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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
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;
|
||||||
|
@ -400,7 +401,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
// if(low_prio_task == LOW_PRIO_TASK_NONE) {
|
// 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, safety_pub, safety, mavlink_fd)) {
|
// if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
|
||||||
// result = VEHICLE_CMD_RESULT_ACCEPTED;
|
// 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_ALTITUDE_CALIBRATION;
|
// low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
|
||||||
|
@ -421,7 +422,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
// if(low_prio_task == LOW_PRIO_TASK_NONE) {
|
// 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, safety_pub, safety, mavlink_fd)) {
|
// if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
|
||||||
// result = VEHICLE_CMD_RESULT_ACCEPTED;
|
// 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_RC_CALIBRATION;
|
// low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
|
||||||
|
@ -441,7 +442,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
if(low_prio_task == LOW_PRIO_TASK_NONE) {
|
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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
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;
|
||||||
|
@ -460,7 +461,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||||
if(low_prio_task == LOW_PRIO_TASK_NONE) {
|
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, safety_pub, safety, mavlink_fd)) {
|
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
|
||||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
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;
|
||||||
|
@ -734,12 +735,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* make sure we are in preflight state */
|
/* make sure we are in preflight state */
|
||||||
memset(¤t_status, 0, sizeof(current_status));
|
memset(¤t_status, 0, sizeof(current_status));
|
||||||
|
|
||||||
/* safety topic */
|
/* armed topic */
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
orb_advert_t safety_pub;
|
orb_advert_t armed_pub;
|
||||||
/* Initialize safety with all false */
|
/* Initialize armed with all false */
|
||||||
memset(&safety, 0, sizeof(safety));
|
memset(&armed, 0, sizeof(armed));
|
||||||
|
|
||||||
|
|
||||||
/* flags for control apps */
|
/* flags for control apps */
|
||||||
struct vehicle_control_mode_s control_mode;
|
struct vehicle_control_mode_s control_mode;
|
||||||
|
@ -768,8 +768,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* set battery warning flag */
|
/* set battery warning flag */
|
||||||
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
|
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
|
||||||
/* set safety device detection flag */
|
|
||||||
|
|
||||||
|
/* set safety device detection flag */
|
||||||
/* XXX do we need this? */
|
/* XXX do we need this? */
|
||||||
//current_status.flag_safety_present = false;
|
//current_status.flag_safety_present = false;
|
||||||
|
|
||||||
|
@ -789,10 +789,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status);
|
orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status);
|
||||||
|
|
||||||
|
|
||||||
safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
|
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||||
|
|
||||||
/* but also subscribe to it */
|
|
||||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
|
||||||
|
|
||||||
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
|
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
|
||||||
|
|
||||||
|
@ -844,6 +841,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* XXX what exactly is this for? */
|
/* XXX what exactly is this for? */
|
||||||
uint64_t last_print_time = 0;
|
uint64_t last_print_time = 0;
|
||||||
|
|
||||||
|
/* Subscribe to safety topic */
|
||||||
|
int safety_sub = orb_subscribe(ORB_ID(safety));
|
||||||
|
struct safety_s safety;
|
||||||
|
memset(&safety, 0, sizeof(safety));
|
||||||
|
|
||||||
/* Subscribe to manual control data */
|
/* Subscribe to manual control data */
|
||||||
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||||
struct manual_control_setpoint_s sp_man;
|
struct manual_control_setpoint_s sp_man;
|
||||||
|
@ -917,7 +919,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* XXX use this! */
|
/* XXX use this! */
|
||||||
//uint64_t failsave_ll_start_time = 0;
|
//uint64_t failsave_ll_start_time = 0;
|
||||||
|
|
||||||
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
|
|
||||||
bool state_changed = true;
|
bool state_changed = true;
|
||||||
bool param_init_forced = true;
|
bool param_init_forced = true;
|
||||||
|
|
||||||
|
@ -936,7 +937,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
|
|
||||||
/* update parameters */
|
/* update parameters */
|
||||||
if (!safety.armed) {
|
if (!armed.armed) {
|
||||||
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
|
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
|
||||||
warnx("failed setting new system type");
|
warnx("failed setting new system type");
|
||||||
}
|
}
|
||||||
|
@ -990,7 +991,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||||
|
|
||||||
/* handle it */
|
/* handle it */
|
||||||
handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety);
|
handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -1003,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
|
orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
|
||||||
|
|
||||||
/* update parameters */
|
/* update parameters */
|
||||||
if (!safety.armed) {
|
if (!armed.armed) {
|
||||||
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
|
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
|
||||||
warnx("failed setting new system type");
|
warnx("failed setting new system type");
|
||||||
}
|
}
|
||||||
|
@ -1028,7 +1029,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
orb_check(safety_sub, &new_data);
|
orb_check(safety_sub, &new_data);
|
||||||
|
|
||||||
if (new_data) {
|
if (new_data) {
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(safety), safety_sub, &safety);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* update global position estimate */
|
/* update global position estimate */
|
||||||
|
@ -1170,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* If in INIT state, try to proceed to STANDBY state */
|
/* If in INIT state, try to proceed to STANDBY state */
|
||||||
if (current_status.arming_state == ARMING_STATE_INIT) {
|
if (current_status.arming_state == ARMING_STATE_INIT) {
|
||||||
// XXX check for sensors
|
// XXX check for sensors
|
||||||
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
|
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
|
||||||
} else {
|
} else {
|
||||||
// XXX: Add emergency stuff if sensors are lost
|
// XXX: Add emergency stuff if sensors are lost
|
||||||
}
|
}
|
||||||
|
@ -1287,7 +1288,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
&& (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk
|
&& (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk
|
||||||
&& !home_position_set
|
&& !home_position_set
|
||||||
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
|
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
|
||||||
&& !safety.armed) {
|
&& !armed.armed) {
|
||||||
warnx("setting home position");
|
warnx("setting home position");
|
||||||
|
|
||||||
/* copy position data to uORB home message, store it locally as well */
|
/* copy position data to uORB home message, store it locally as well */
|
||||||
|
@ -1579,7 +1580,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
|
mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
|
||||||
tune_negative();
|
tune_negative();
|
||||||
} else {
|
} else {
|
||||||
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
|
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
|
||||||
tune_positive();
|
tune_positive();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1595,13 +1596,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* check if left stick is in lower right position --> arm */
|
/* check if left stick is in lower right position and we're in manual --> arm */
|
||||||
if (current_status.flag_control_manual_enabled &&
|
if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled &&
|
||||||
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
|
|
||||||
sp_man.yaw > STICK_ON_OFF_LIMIT &&
|
sp_man.yaw > STICK_ON_OFF_LIMIT &&
|
||||||
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||||
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
|
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd);
|
||||||
stick_on_counter = 0;
|
stick_on_counter = 0;
|
||||||
tune_positive();
|
tune_positive();
|
||||||
|
|
||||||
|
@ -1704,13 +1704,13 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
// XXX check if this is correct
|
// XXX check if this is correct
|
||||||
/* arm / disarm on request */
|
/* arm / disarm on request */
|
||||||
if (sp_offboard.armed && !safety.armed) {
|
if (sp_offboard.armed && !armed.armed) {
|
||||||
|
|
||||||
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
|
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd);
|
||||||
|
|
||||||
} else if (!sp_offboard.armed && safety.armed) {
|
} else if (!sp_offboard.armed && armed.armed) {
|
||||||
|
|
||||||
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
|
arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1772,7 +1772,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* play tone according to evaluation result */
|
/* play tone according to evaluation result */
|
||||||
/* check if we recently armed */
|
/* check if we recently armed */
|
||||||
if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
|
if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
|
||||||
if (tune_arm() == OK)
|
if (tune_arm() == OK)
|
||||||
arm_tune_played = true;
|
arm_tune_played = true;
|
||||||
|
|
||||||
|
@ -1789,7 +1789,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
/* reset arm_tune_played when disarmed */
|
/* reset arm_tune_played when disarmed */
|
||||||
if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
|
if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
|
||||||
arm_tune_played = false;
|
arm_tune_played = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1812,6 +1812,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
close(sp_offboard_sub);
|
close(sp_offboard_sub);
|
||||||
close(global_position_sub);
|
close(global_position_sub);
|
||||||
close(sensor_sub);
|
close(sensor_sub);
|
||||||
|
close(safety_sub);
|
||||||
close(cmd_sub);
|
close(cmd_sub);
|
||||||
|
|
||||||
warnx("exiting");
|
warnx("exiting");
|
||||||
|
|
|
@ -43,7 +43,7 @@
|
||||||
|
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -57,7 +57,7 @@
|
||||||
#include "commander_helper.h"
|
#include "commander_helper.h"
|
||||||
|
|
||||||
|
|
||||||
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
|
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) {
|
||||||
|
|
||||||
|
|
||||||
int ret = ERROR;
|
int ret = ERROR;
|
||||||
|
@ -73,8 +73,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
/* allow going back from INIT for calibration */
|
/* allow going back from INIT for calibration */
|
||||||
if (current_state->arming_state == ARMING_STATE_STANDBY) {
|
if (current_state->arming_state == ARMING_STATE_STANDBY) {
|
||||||
ret = OK;
|
ret = OK;
|
||||||
safety->armed = false;
|
armed->armed = false;
|
||||||
safety->ready_to_arm = false;
|
armed->ready_to_arm = false;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case ARMING_STATE_STANDBY:
|
case ARMING_STATE_STANDBY:
|
||||||
|
@ -86,8 +86,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
/* sensors need to be initialized for STANDBY state */
|
/* sensors need to be initialized for STANDBY state */
|
||||||
if (current_state->condition_system_sensors_initialized) {
|
if (current_state->condition_system_sensors_initialized) {
|
||||||
ret = OK;
|
ret = OK;
|
||||||
safety->armed = false;
|
armed->armed = false;
|
||||||
safety->ready_to_arm = true;
|
armed->ready_to_arm = true;
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
|
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
|
||||||
}
|
}
|
||||||
|
@ -101,7 +101,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
|
|
||||||
/* XXX conditions for arming? */
|
/* XXX conditions for arming? */
|
||||||
ret = OK;
|
ret = OK;
|
||||||
safety->armed = true;
|
armed->armed = true;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case ARMING_STATE_ARMED_ERROR:
|
case ARMING_STATE_ARMED_ERROR:
|
||||||
|
@ -111,7 +111,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
|
|
||||||
/* XXX conditions for an error state? */
|
/* XXX conditions for an error state? */
|
||||||
ret = OK;
|
ret = OK;
|
||||||
safety->armed = true;
|
armed->armed = true;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case ARMING_STATE_STANDBY_ERROR:
|
case ARMING_STATE_STANDBY_ERROR:
|
||||||
|
@ -120,8 +120,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
|| current_state->arming_state == ARMING_STATE_INIT
|
|| current_state->arming_state == ARMING_STATE_INIT
|
||||||
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
|
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||||
ret = OK;
|
ret = OK;
|
||||||
safety->armed = false;
|
armed->armed = false;
|
||||||
safety->ready_to_arm = false;
|
armed->ready_to_arm = false;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case ARMING_STATE_REBOOT:
|
case ARMING_STATE_REBOOT:
|
||||||
|
@ -132,8 +132,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
|| current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
|
|| current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
|
||||||
|
|
||||||
ret = OK;
|
ret = OK;
|
||||||
safety->armed = false;
|
armed->armed = false;
|
||||||
safety->ready_to_arm = false;
|
armed->ready_to_arm = false;
|
||||||
|
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -151,8 +151,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
||||||
current_state->timestamp = hrt_absolute_time();
|
current_state->timestamp = hrt_absolute_time();
|
||||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
|
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
|
||||||
|
|
||||||
safety->timestamp = hrt_absolute_time();
|
armed->timestamp = hrt_absolute_time();
|
||||||
orb_publish(ORB_ID(actuator_safety), safety_pub, safety);
|
orb_publish(ORB_ID(actuator_armed), armed_pub, armed);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -46,11 +46,11 @@
|
||||||
|
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
|
||||||
|
|
||||||
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd);
|
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd);
|
||||||
|
|
||||||
int 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);
|
||||||
|
|
||||||
|
|
|
@ -51,7 +51,7 @@
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <poll.h>
|
#include <poll.h>
|
||||||
#include <drivers/drv_gpio.h>
|
#include <drivers/drv_gpio.h>
|
||||||
#include <modules/px4iofirmware/protocol.h>
|
#include <modules/px4iofirmware/protocol.h>
|
||||||
|
@ -63,8 +63,8 @@ struct gpio_led_s {
|
||||||
int pin;
|
int pin;
|
||||||
struct vehicle_status_s status;
|
struct vehicle_status_s status;
|
||||||
int vehicle_status_sub;
|
int vehicle_status_sub;
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
int actuator_safety_sub;
|
int actuator_armed_sub;
|
||||||
bool led_state;
|
bool led_state;
|
||||||
int counter;
|
int counter;
|
||||||
};
|
};
|
||||||
|
@ -233,12 +233,12 @@ void gpio_led_cycle(FAR void *arg)
|
||||||
orb_check(priv->vehicle_status_sub, &status_updated);
|
orb_check(priv->vehicle_status_sub, &status_updated);
|
||||||
|
|
||||||
if (status_updated)
|
if (status_updated)
|
||||||
orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety);
|
orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
|
||||||
|
|
||||||
/* select pattern for current status */
|
/* select pattern for current status */
|
||||||
int pattern = 0;
|
int pattern = 0;
|
||||||
|
|
||||||
if (priv->safety.armed) {
|
if (priv->armed.armed) {
|
||||||
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
|
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
|
||||||
pattern = 0x3f; // ****** solid (armed)
|
pattern = 0x3f; // ****** solid (armed)
|
||||||
|
|
||||||
|
@ -247,10 +247,10 @@ void gpio_led_cycle(FAR void *arg)
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (priv->safety.ready_to_arm) {
|
if (priv->armed.ready_to_arm) {
|
||||||
pattern = 0x00; // ______ off (disarmed, preflight check)
|
pattern = 0x00; // ______ off (disarmed, preflight check)
|
||||||
|
|
||||||
} else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
|
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
|
||||||
pattern = 0x38; // ***___ slow blink (disarmed, ready)
|
pattern = 0x38; // ***___ slow blink (disarmed, ready)
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos;
|
||||||
struct vehicle_status_s v_status;
|
struct vehicle_status_s v_status;
|
||||||
struct rc_channels_s rc;
|
struct rc_channels_s rc;
|
||||||
struct rc_input_values rc_raw;
|
struct rc_input_values rc_raw;
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
struct actuator_controls_effective_s actuators_0;
|
struct actuator_controls_effective_s actuators_0;
|
||||||
struct vehicle_attitude_s att;
|
struct vehicle_attitude_s att;
|
||||||
|
|
||||||
|
@ -110,7 +110,7 @@ static void l_global_position_setpoint(const struct listener *l);
|
||||||
static void l_local_position_setpoint(const struct listener *l);
|
static void l_local_position_setpoint(const struct listener *l);
|
||||||
static void l_attitude_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_outputs(const struct listener *l);
|
||||||
static void l_actuator_safety(const struct listener *l);
|
static void l_actuator_armed(const struct listener *l);
|
||||||
static void l_manual_control_setpoint(const struct listener *l);
|
static void l_manual_control_setpoint(const struct listener *l);
|
||||||
static void l_vehicle_attitude_controls(const struct listener *l);
|
static void l_vehicle_attitude_controls(const struct listener *l);
|
||||||
static void l_debug_key_value(const struct listener *l);
|
static void l_debug_key_value(const struct listener *l);
|
||||||
|
@ -135,7 +135,7 @@ static const struct listener listeners[] = {
|
||||||
{l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
|
{l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
|
||||||
{l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
|
{l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
|
||||||
{l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
|
{l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
|
||||||
{l_actuator_safety, &mavlink_subs.safety_sub, 0},
|
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
|
||||||
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
|
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
|
||||||
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
|
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
|
||||||
{l_debug_key_value, &mavlink_subs.debug_key_value, 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 */
|
/* immediately communicate state changes back to user */
|
||||||
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
|
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
|
||||||
orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
|
||||||
|
|
||||||
/* enable or disable HIL */
|
/* enable or disable HIL */
|
||||||
set_hil_on_off(v_status.flag_hil_enabled);
|
set_hil_on_off(v_status.flag_hil_enabled);
|
||||||
|
@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l)
|
||||||
act_outputs.output[7]);
|
act_outputs.output[7]);
|
||||||
|
|
||||||
/* only send in HIL mode */
|
/* only send in HIL mode */
|
||||||
if (mavlink_hil_enabled && safety.armed) {
|
if (mavlink_hil_enabled && armed.armed) {
|
||||||
|
|
||||||
/* translate the current syste state to mavlink state and mode */
|
/* translate the current syste state to mavlink state and mode */
|
||||||
uint8_t mavlink_state = 0;
|
uint8_t mavlink_state = 0;
|
||||||
|
@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
l_actuator_safety(const struct listener *l)
|
l_actuator_armed(const struct listener *l)
|
||||||
{
|
{
|
||||||
orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -759,8 +759,8 @@ uorb_receive_start(void)
|
||||||
orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
|
orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
|
||||||
|
|
||||||
/* --- ACTUATOR ARMED VALUE --- */
|
/* --- ACTUATOR ARMED VALUE --- */
|
||||||
mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */
|
orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */
|
||||||
|
|
||||||
/* --- MAPPED MANUAL CONTROL INPUTS --- */
|
/* --- MAPPED MANUAL CONTROL INPUTS --- */
|
||||||
mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||||
|
|
|
@ -60,7 +60,7 @@
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_controls_effective.h>
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
#include <uORB/topics/telemetry_status.h>
|
#include <uORB/topics/telemetry_status.h>
|
||||||
#include <uORB/topics/debug_key_value.h>
|
#include <uORB/topics/debug_key_value.h>
|
||||||
|
@ -79,6 +79,7 @@ struct mavlink_subscriptions {
|
||||||
int man_control_sp_sub;
|
int man_control_sp_sub;
|
||||||
int safety_sub;
|
int safety_sub;
|
||||||
int actuators_sub;
|
int actuators_sub;
|
||||||
|
int armed_sub;
|
||||||
int local_pos_sub;
|
int local_pos_sub;
|
||||||
int spa_sub;
|
int spa_sub;
|
||||||
int spl_sub;
|
int spl_sub;
|
||||||
|
|
|
@ -273,7 +273,7 @@ void mavlink_update_system(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety,
|
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
|
||||||
uint8_t *mavlink_state, uint8_t *mavlink_mode)
|
uint8_t *mavlink_state, uint8_t *mavlink_mode)
|
||||||
{
|
{
|
||||||
/* reset MAVLink mode bitfield */
|
/* reset MAVLink mode bitfield */
|
||||||
|
@ -284,12 +284,12 @@ get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, co
|
||||||
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (safety->hil_enabled) {
|
if (control_mode->flag_system_hil_enabled) {
|
||||||
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* set arming state */
|
/* set arming state */
|
||||||
if (safety->armed) {
|
if (armed->armed) {
|
||||||
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||||
} else {
|
} else {
|
||||||
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
|
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
|
||||||
|
@ -370,7 +370,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||||
/* XXX this is never written? */
|
/* XXX this is never written? */
|
||||||
struct vehicle_status_s v_status;
|
struct vehicle_status_s v_status;
|
||||||
struct vehicle_control_mode_s control_mode;
|
struct vehicle_control_mode_s control_mode;
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
|
|
||||||
/* work around some stupidity in task_create's argv handling */
|
/* work around some stupidity in task_create's argv handling */
|
||||||
argc -= 2;
|
argc -= 2;
|
||||||
|
@ -438,7 +438,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||||
/* translate the current system state to mavlink state and mode */
|
/* translate the current system state to mavlink state and mode */
|
||||||
uint8_t mavlink_state = 0;
|
uint8_t mavlink_state = 0;
|
||||||
uint8_t mavlink_mode = 0;
|
uint8_t mavlink_mode = 0;
|
||||||
get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode);
|
get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
|
||||||
|
|
||||||
/* send heartbeat */
|
/* send heartbeat */
|
||||||
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);
|
||||||
|
|
|
@ -56,7 +56,7 @@
|
||||||
#include <uORB/topics/optical_flow.h>
|
#include <uORB/topics/optical_flow.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
#include <uORB/topics/debug_key_value.h>
|
#include <uORB/topics/debug_key_value.h>
|
||||||
#include <drivers/drv_rc_input.h>
|
#include <drivers/drv_rc_input.h>
|
||||||
|
|
|
@ -51,5 +51,5 @@ extern volatile bool thread_should_exit;
|
||||||
* Translate the custom state into standard mavlink modes and state.
|
* Translate the custom state into standard mavlink modes and state.
|
||||||
*/
|
*/
|
||||||
extern void
|
extern void
|
||||||
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety,
|
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
|
||||||
uint8_t *mavlink_state, uint8_t *mavlink_mode);
|
uint8_t *mavlink_state, uint8_t *mavlink_mode);
|
||||||
|
|
|
@ -58,7 +58,7 @@
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <drivers/drv_gyro.h>
|
#include <drivers/drv_gyro.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
@ -94,8 +94,8 @@ mc_thread_main(int argc, char *argv[])
|
||||||
/* declare and safely initialize all structs */
|
/* declare and safely initialize all structs */
|
||||||
struct vehicle_control_mode_s control_mode;
|
struct vehicle_control_mode_s control_mode;
|
||||||
memset(&control_mode, 0, sizeof(control_mode));
|
memset(&control_mode, 0, sizeof(control_mode));
|
||||||
struct actuator_safety_s safety;
|
struct actuator_armed_s armed;
|
||||||
memset(&safety, 0, sizeof(safety));
|
memset(&armed, 0, sizeof(armed));
|
||||||
struct vehicle_attitude_s att;
|
struct vehicle_attitude_s att;
|
||||||
memset(&att, 0, sizeof(att));
|
memset(&att, 0, sizeof(att));
|
||||||
struct vehicle_attitude_setpoint_s att_sp;
|
struct vehicle_attitude_setpoint_s att_sp;
|
||||||
|
@ -121,7 +121,7 @@ mc_thread_main(int argc, char *argv[])
|
||||||
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||||
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||||
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||||
|
|
||||||
|
@ -210,10 +210,10 @@ mc_thread_main(int argc, char *argv[])
|
||||||
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
||||||
}
|
}
|
||||||
|
|
||||||
orb_check(safety_sub, &updated);
|
orb_check(armed_sub, &updated);
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
|
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* get a local copy of manual setpoint */
|
/* get a local copy of manual setpoint */
|
||||||
|
@ -261,7 +261,7 @@ mc_thread_main(int argc, char *argv[])
|
||||||
/* initialize to current yaw if switching to manual or att control */
|
/* initialize to current yaw if switching to manual or att control */
|
||||||
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
||||||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
|
control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||||
safety.armed != flag_armed) {
|
armed.armed != flag_armed) {
|
||||||
att_sp.yaw_body = att.yaw;
|
att_sp.yaw_body = att.yaw;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -275,7 +275,7 @@ mc_thread_main(int argc, char *argv[])
|
||||||
att_sp.pitch_body = manual.pitch;
|
att_sp.pitch_body = manual.pitch;
|
||||||
|
|
||||||
/* set attitude if arming */
|
/* set attitude if arming */
|
||||||
if (!flag_control_attitude_enabled && safety.armed) {
|
if (!flag_control_attitude_enabled && armed.armed) {
|
||||||
att_sp.yaw_body = att.yaw;
|
att_sp.yaw_body = att.yaw;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -366,7 +366,7 @@ mc_thread_main(int argc, char *argv[])
|
||||||
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
|
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
|
||||||
flag_control_position_enabled = control_mode.flag_control_position_enabled;
|
flag_control_position_enabled = control_mode.flag_control_position_enabled;
|
||||||
flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
|
flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
|
||||||
flag_armed = safety.armed;
|
flag_armed = armed.armed;
|
||||||
|
|
||||||
perf_end(mc_loop_perf);
|
perf_end(mc_loop_perf);
|
||||||
} /* end of poll call for attitude updates */
|
} /* end of poll call for attitude updates */
|
||||||
|
|
|
@ -60,7 +60,7 @@
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/actuator_safety.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.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>
|
||||||
|
@ -194,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new);
|
||||||
|
|
||||||
static void handle_command(struct vehicle_command_s *cmd);
|
static void handle_command(struct vehicle_command_s *cmd);
|
||||||
|
|
||||||
static void handle_status(struct actuator_safety_s *safety);
|
static void handle_status(struct actuator_armed_s *armed);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Create folder for current logging session. Store folder name in 'log_folder'.
|
* Create folder for current logging session. Store folder name in 'log_folder'.
|
||||||
|
@ -628,8 +628,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
struct vehicle_status_s buf_status;
|
struct vehicle_status_s buf_status;
|
||||||
memset(&buf_status, 0, sizeof(buf_status));
|
memset(&buf_status, 0, sizeof(buf_status));
|
||||||
|
|
||||||
struct actuator_safety_s buf_safety;
|
struct actuator_armed_s buf_armed;
|
||||||
memset(&buf_safety, 0, sizeof(buf_safety));
|
memset(&buf_armed, 0, sizeof(buf_armed));
|
||||||
|
|
||||||
/* warning! using union here to save memory, elements should be used separately! */
|
/* warning! using union here to save memory, elements should be used separately! */
|
||||||
union {
|
union {
|
||||||
|
@ -659,7 +659,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
struct {
|
struct {
|
||||||
int cmd_sub;
|
int cmd_sub;
|
||||||
int status_sub;
|
int status_sub;
|
||||||
int safety_sub;
|
int armed_sub;
|
||||||
int sensor_sub;
|
int sensor_sub;
|
||||||
int att_sub;
|
int att_sub;
|
||||||
int att_sp_sub;
|
int att_sp_sub;
|
||||||
|
@ -717,9 +717,9 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
fds[fdsc_count].events = POLLIN;
|
fds[fdsc_count].events = POLLIN;
|
||||||
fdsc_count++;
|
fdsc_count++;
|
||||||
|
|
||||||
/* --- SAFETY --- */
|
/* --- ACTUATOR ARMED --- */
|
||||||
subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
fds[fdsc_count].fd = subs.safety_sub;
|
fds[fdsc_count].fd = subs.armed_sub;
|
||||||
fds[fdsc_count].events = POLLIN;
|
fds[fdsc_count].events = POLLIN;
|
||||||
fdsc_count++;
|
fdsc_count++;
|
||||||
|
|
||||||
|
@ -896,12 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
handled_topics++;
|
handled_topics++;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* --- SAFETY- LOG MANAGEMENT --- */
|
/* --- ARMED- LOG MANAGEMENT --- */
|
||||||
if (fds[ifds++].revents & POLLIN) {
|
if (fds[ifds++].revents & POLLIN) {
|
||||||
orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety);
|
orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
|
||||||
|
|
||||||
if (log_when_armed) {
|
if (log_when_armed) {
|
||||||
handle_status(&buf_safety);
|
handle_status(&buf_armed);
|
||||||
}
|
}
|
||||||
|
|
||||||
handled_topics++;
|
handled_topics++;
|
||||||
|
@ -912,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
|
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
|
||||||
|
|
||||||
//if (log_when_armed) {
|
//if (log_when_armed) {
|
||||||
// handle_status(&buf_safety);
|
// handle_status(&buf_armed);
|
||||||
//}
|
//}
|
||||||
|
|
||||||
//handled_topics++;
|
//handled_topics++;
|
||||||
|
@ -944,7 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
log_msg.body.log_STAT.flight_mode = 0;
|
log_msg.body.log_STAT.flight_mode = 0;
|
||||||
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_safety.armed; /* XXX fmu armed correct? */
|
log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
|
||||||
log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
|
log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
|
||||||
log_msg.body.log_STAT.battery_current = buf_status.current_battery;
|
log_msg.body.log_STAT.battery_current = buf_status.current_battery;
|
||||||
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
|
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
|
||||||
|
@ -1348,10 +1348,10 @@ void handle_command(struct vehicle_command_s *cmd)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void handle_status(struct actuator_safety_s *safety)
|
void handle_status(struct actuator_armed_s *armed)
|
||||||
{
|
{
|
||||||
if (safety->armed != flag_system_armed) {
|
if (armed->armed != flag_system_armed) {
|
||||||
flag_system_armed = safety->armed;
|
flag_system_armed = armed->armed;
|
||||||
|
|
||||||
if (flag_system_armed) {
|
if (flag_system_armed) {
|
||||||
sdlog2_start_log();
|
sdlog2_start_log();
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s);
|
||||||
#include "topics/vehicle_status.h"
|
#include "topics/vehicle_status.h"
|
||||||
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
|
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
|
||||||
|
|
||||||
|
#include "topics/safety.h"
|
||||||
|
ORB_DEFINE(safety, struct safety_s);
|
||||||
|
|
||||||
#include "topics/battery_status.h"
|
#include "topics/battery_status.h"
|
||||||
ORB_DEFINE(battery_status, struct battery_status_s);
|
ORB_DEFINE(battery_status, struct battery_status_s);
|
||||||
|
|
||||||
|
@ -153,8 +156,8 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
|
||||||
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
|
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
|
||||||
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
|
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
|
||||||
|
|
||||||
#include "topics/actuator_safety.h"
|
#include "topics/actuator_armed.h"
|
||||||
ORB_DEFINE(actuator_safety, struct actuator_safety_s);
|
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
|
||||||
|
|
||||||
/* actuator controls, as set by actuators / mixers after limiting */
|
/* actuator controls, as set by actuators / mixers after limiting */
|
||||||
#include "topics/actuator_controls_effective.h"
|
#include "topics/actuator_controls_effective.h"
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -32,35 +32,27 @@
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file actuator_controls.h
|
* @file actuator_armed.h
|
||||||
*
|
*
|
||||||
* Actuator control topics - mixer inputs.
|
* Actuator armed topic
|
||||||
*
|
*
|
||||||
* 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
|
#ifndef TOPIC_ACTUATOR_ARMED_H
|
||||||
#define TOPIC_ACTUATOR_SAFETY_H
|
#define TOPIC_ACTUATOR_ARMED_H
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include "../uORB.h"
|
#include "../uORB.h"
|
||||||
|
|
||||||
/** global 'actuator output is live' control. */
|
/** global 'actuator output is live' control. */
|
||||||
struct actuator_safety_s {
|
struct actuator_armed_s {
|
||||||
|
|
||||||
uint64_t timestamp;
|
uint64_t timestamp;
|
||||||
bool safety_switch_available; /**< Set to true if a safety switch is connected */
|
|
||||||
bool safety_off; /**< Set to true if safety is off */
|
|
||||||
bool armed; /**< Set to true if system is armed */
|
bool armed; /**< Set to true if system is armed */
|
||||||
bool ready_to_arm; /**< Set to true if system is ready to be armed */
|
bool ready_to_arm; /**< Set to true if system is ready to be armed */
|
||||||
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
|
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
|
||||||
bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */
|
|
||||||
};
|
};
|
||||||
|
|
||||||
ORB_DECLARE(actuator_safety);
|
ORB_DECLARE(actuator_armed);
|
||||||
|
|
||||||
#endif
|
#endif
|
|
@ -0,0 +1,57 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file safety.h
|
||||||
|
*
|
||||||
|
* Safety topic to pass safety state from px4io driver to commander
|
||||||
|
* This concerns only the safety button of the px4io but has nothing to do
|
||||||
|
* with arming/disarming.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef TOPIC_SAFETY_H
|
||||||
|
#define TOPIC_SAFETY_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "../uORB.h"
|
||||||
|
|
||||||
|
struct safety_s {
|
||||||
|
|
||||||
|
uint64_t timestamp;
|
||||||
|
bool safety_switch_available; /**< Set to true if a safety switch is connected */
|
||||||
|
bool safety_off; /**< Set to true if safety is off */
|
||||||
|
};
|
||||||
|
|
||||||
|
ORB_DECLARE(safety);
|
||||||
|
|
||||||
|
#endif
|
|
@ -69,9 +69,13 @@ struct vehicle_control_mode_s
|
||||||
bool flag_system_emergency;
|
bool flag_system_emergency;
|
||||||
bool flag_preflight_calibration;
|
bool flag_preflight_calibration;
|
||||||
|
|
||||||
|
// XXX needs yet to be set by state machine helper
|
||||||
|
bool flag_system_hil_enabled;
|
||||||
|
|
||||||
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
|
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
|
||||||
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
|
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
|
||||||
bool flag_auto_enabled;
|
// XXX shouldn't be necessairy
|
||||||
|
//bool flag_auto_enabled;
|
||||||
bool flag_control_rates_enabled; /**< true if rates are stabilized */
|
bool flag_control_rates_enabled; /**< true if rates are stabilized */
|
||||||
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
|
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_velocity_enabled; /**< true if speed (implies direction) is controlled */
|
||||||
|
|
Loading…
Reference in New Issue