fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 13:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Renamed actuator_safety back to actuator_armed, compiling but untested

Browse Source
This commit is contained in:
Julian Oes
2013-07-15 22:15:15 +02:00
parent bf2ff98856
commit 1b38cf715d
25 changed files with 316 additions and 243 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.c
+44 -43
View File
@@ -76,9 +76,10 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <mavlink/mavlink_log.h>
#include <drivers/drv_led.h>
@@ -177,7 +178,7 @@ int led_off(int led);
void do_reboot(void);
void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety);
void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed);
// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
@@ -253,7 +254,7 @@ void do_reboot()
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety)
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed)
{
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
@@ -273,13 +274,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
} else {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
@@ -292,14 +293,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* request to arm */
if ((int)cmd->param1 == 1) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
/* request to disarm */
} else if ((int)cmd->param1 == 0) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
@@ -312,7 +313,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* request for an autopilot reboot */
if ((int)cmd->param1 == 1) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) {
/* reboot is executed later, after positive tune is sent */
result = VEHICLE_CMD_RESULT_ACCEPTED;
tune_positive();
@@ -361,7 +362,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
@@ -380,7 +381,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
@@ -400,7 +401,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
// if(low_prio_task == LOW_PRIO_TASK_NONE) {
// /* try to go to INIT/PREFLIGHT arming state */
// if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
// if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
// result = VEHICLE_CMD_RESULT_ACCEPTED;
// /* now set the task for the low prio thread */
// low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
@@ -421,7 +422,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
// if(low_prio_task == LOW_PRIO_TASK_NONE) {
// /* try to go to INIT/PREFLIGHT arming state */
// if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
// if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
// result = VEHICLE_CMD_RESULT_ACCEPTED;
// /* now set the task for the low prio thread */
// low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
@@ -441,7 +442,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
@@ -460,7 +461,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if(low_prio_task == LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
/* now set the task for the low prio thread */
low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
@@ -734,12 +735,11 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
/* safety topic */
struct actuator_safety_s safety;
orb_advert_t safety_pub;
/* Initialize safety with all false */
memset(&safety, 0, sizeof(safety));
/* armed topic */
struct actuator_armed_s armed;
orb_advert_t armed_pub;
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
@@ -768,8 +768,8 @@ int commander_thread_main(int argc, char *argv[])
/* set battery warning flag */
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
/* set safety device detection flag */
/* set safety device detection flag */
/* XXX do we need this? */
//current_status.flag_safety_present = false;
@@ -789,10 +789,7 @@ int commander_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_status), status_pub, &current_status);
safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
/* but also subscribe to it */
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
@@ -844,6 +841,11 @@ int commander_thread_main(int argc, char *argv[])
/* XXX what exactly is this for? */
uint64_t last_print_time = 0;
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
struct safety_s safety;
memset(&safety, 0, sizeof(safety));
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
@@ -917,7 +919,6 @@ int commander_thread_main(int argc, char *argv[])
/* XXX use this! */
//uint64_t failsave_ll_start_time = 0;
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
bool state_changed = true;
bool param_init_forced = true;
@@ -936,7 +937,7 @@ int commander_thread_main(int argc, char *argv[])
/* update parameters */
if (!safety.armed) {
if (!armed.armed) {
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
warnx("failed setting new system type");
}
@@ -990,7 +991,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
handle_command(status_pub, &current_status, &cmd, safety_pub, &safety);
handle_command(status_pub, &current_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, &param_changed);
/* update parameters */
if (!safety.armed) {
if (!armed.armed) {
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
warnx("failed setting new system type");
}
@@ -1028,7 +1029,7 @@ int commander_thread_main(int argc, char *argv[])
orb_check(safety_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
orb_copy(ORB_ID(safety), safety_sub, &safety);
}
/* update global position estimate */
@@ -1170,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (current_status.arming_state == ARMING_STATE_INIT) {
// XXX check for sensors
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
} else {
// XXX: Add emergency stuff if sensors are lost
}
@@ -1287,7 +1288,7 @@ int commander_thread_main(int argc, char *argv[])
&& (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk
&& !home_position_set
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
&& !safety.armed) {
&& !armed.armed) {
warnx("setting home position");
/* copy position data to uORB home message, store it locally as well */
@@ -1579,7 +1580,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
tune_negative();
} else {
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
tune_positive();
}
@@ -1595,13 +1596,12 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check if left stick is in lower right position --> arm */
if (current_status.flag_control_manual_enabled &&
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
/* check if left stick is in lower right position and we're in manual --> arm */
if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled &&
sp_man.yaw > STICK_ON_OFF_LIMIT &&
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd);
stick_on_counter = 0;
tune_positive();
@@ -1704,13 +1704,13 @@ int commander_thread_main(int argc, char *argv[])
// XXX check if this is correct
/* arm / disarm on request */
if (sp_offboard.armed && !safety.armed) {
if (sp_offboard.armed && !armed.armed) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
arming_state_transition(status_pub, &current_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, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
}
} else {
@@ -1772,7 +1772,7 @@ int commander_thread_main(int argc, char *argv[])
/* play tone according to evaluation result */
/* check if we recently armed */
if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
if (tune_arm() == OK)
arm_tune_played = true;
@@ -1789,7 +1789,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* reset arm_tune_played when disarmed */
if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
arm_tune_played = false;
}
@@ -1812,6 +1812,7 @@ int commander_thread_main(int argc, char *argv[])
close(sp_offboard_sub);
close(global_position_sub);
close(sensor_sub);
close(safety_sub);
close(cmd_sub);
warnx("exiting");
src/modules/commander/commander_helper.h
+1 -1
View File
@@ -43,7 +43,7 @@
#include <uORB/uORB.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>
src/modules/commander/state_machine_helper.c
+13 -13
View File
@@ -57,7 +57,7 @@
#include "commander_helper.h"
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) {
int ret = ERROR;
@@ -73,8 +73,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* allow going back from INIT for calibration */
if (current_state->arming_state == ARMING_STATE_STANDBY) {
ret = OK;
safety->armed = false;
safety->ready_to_arm = false;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_STANDBY:
@@ -86,8 +86,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* sensors need to be initialized for STANDBY state */
if (current_state->condition_system_sensors_initialized) {
ret = OK;
safety->armed = false;
safety->ready_to_arm = true;
armed->armed = false;
armed->ready_to_arm = true;
} else {
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
}
@@ -101,7 +101,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* XXX conditions for arming? */
ret = OK;
safety->armed = true;
armed->armed = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
@@ -111,7 +111,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* XXX conditions for an error state? */
ret = OK;
safety->armed = true;
armed->armed = true;
}
break;
case ARMING_STATE_STANDBY_ERROR:
@@ -120,8 +120,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|| current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = OK;
safety->armed = false;
safety->ready_to_arm = false;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
@@ -132,8 +132,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|| current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = OK;
safety->armed = false;
safety->ready_to_arm = false;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
@@ -151,8 +151,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
current_state->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
safety->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_safety), safety_pub, safety);
armed->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_armed), armed_pub, armed);
}
}
src/modules/commander/state_machine_helper.h
+2 -2
View File
@@ -46,11 +46,11 @@
#include <uORB/uORB.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>
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);
src/modules/gpio_led/gpio_led.c
+7 -7
View File
@@ -51,7 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/actuator_armed.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -63,8 +63,8 @@ struct gpio_led_s {
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
struct actuator_safety_s safety;
int actuator_safety_sub;
struct actuator_armed_s armed;
int actuator_armed_sub;
bool led_state;
int counter;
};
@@ -233,12 +233,12 @@ void gpio_led_cycle(FAR void *arg)
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety);
orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
/* select pattern for current status */
int pattern = 0;
if (priv->safety.armed) {
if (priv->armed.armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x3f; // ****** solid (armed)
@@ -247,10 +247,10 @@ void gpio_led_cycle(FAR void *arg)
}
} else {
if (priv->safety.ready_to_arm) {
if (priv->armed.ready_to_arm) {
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
src/modules/mavlink/orb_listener.c
+9 -9
View File
@@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos;
struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_safety_s safety;
struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_0;
struct vehicle_attitude_s att;
@@ -110,7 +110,7 @@ static void l_global_position_setpoint(const struct listener *l);
static void l_local_position_setpoint(const struct listener *l);
static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
static void l_actuator_safety(const struct listener *l);
static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
@@ -135,7 +135,7 @@ static const struct listener listeners[] = {
{l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
{l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
{l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
{l_actuator_safety, &mavlink_subs.safety_sub, 0},
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
@@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l)
{
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
@@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l)
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_hil_enabled && safety.armed) {
if (mavlink_hil_enabled && armed.armed) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l)
}
void
l_actuator_safety(const struct listener *l)
l_actuator_armed(const struct listener *l)
{
orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
}
void
@@ -759,8 +759,8 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
/* --- ACTUATOR ARMED VALUE --- */
mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */
mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */
/* --- MAPPED MANUAL CONTROL INPUTS --- */
mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
src/modules/mavlink/orb_topics.h
+2 -1
View File
@@ -60,7 +60,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
@@ -79,6 +79,7 @@ struct mavlink_subscriptions {
int man_control_sp_sub;
int safety_sub;
int actuators_sub;
int armed_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
src/modules/mavlink_onboard/mavlink.c
+5 -5
View File
@@ -273,7 +273,7 @@ void mavlink_update_system(void)
}
void
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety,
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode)
{
/* reset MAVLink mode bitfield */
@@ -284,12 +284,12 @@ get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, co
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
if (safety->hil_enabled) {
if (control_mode->flag_system_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* set arming state */
if (safety->armed) {
if (armed->armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
@@ -370,7 +370,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* XXX this is never written? */
struct vehicle_status_s v_status;
struct vehicle_control_mode_s control_mode;
struct actuator_safety_s safety;
struct actuator_armed_s armed;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
@@ -438,7 +438,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode);
get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
src/modules/mavlink_onboard/orb_topics.h
+1 -1
View File
@@ -56,7 +56,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
src/modules/mavlink_onboard/util.h
+1 -1
View File
@@ -51,5 +51,5 @@ extern volatile bool thread_should_exit;
* Translate the custom state into standard mavlink modes and state.
*/
extern void
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety,
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode);
src/modules/multirotor_att_control/multirotor_att_control_main.c
+9 -9
View File
@@ -58,7 +58,7 @@
#include <uORB/uORB.h>
#include <drivers/drv_gyro.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_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 */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct actuator_safety_s safety;
memset(&safety, 0, sizeof(safety));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -121,7 +121,7 @@ mc_thread_main(int argc, char *argv[])
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -210,10 +210,10 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(safety_sub, &updated);
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* get a local copy of manual setpoint */
@@ -261,7 +261,7 @@ mc_thread_main(int argc, char *argv[])
/* initialize to current yaw if switching to manual or att control */
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
safety.armed != flag_armed) {
armed.armed != flag_armed) {
att_sp.yaw_body = att.yaw;
}
@@ -275,7 +275,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = manual.pitch;
/* set attitude if arming */
if (!flag_control_attitude_enabled && safety.armed) {
if (!flag_control_attitude_enabled && armed.armed) {
att_sp.yaw_body = att.yaw;
}
@@ -366,7 +366,7 @@ mc_thread_main(int argc, char *argv[])
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
flag_control_position_enabled = control_mode.flag_control_position_enabled;
flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
flag_armed = safety.armed;
flag_armed = armed.armed;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
src/modules/sdlog2/sdlog2.c
+16 -16
View File
@@ -60,7 +60,7 @@
#include <drivers/drv_hrt.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/sensor_combined.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_status(struct actuator_safety_s *safety);
static void handle_status(struct actuator_armed_s *armed);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
@@ -628,8 +628,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
struct actuator_safety_s buf_safety;
memset(&buf_safety, 0, sizeof(buf_safety));
struct actuator_armed_s buf_armed;
memset(&buf_armed, 0, sizeof(buf_armed));
/* warning! using union here to save memory, elements should be used separately! */
union {
@@ -659,7 +659,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct {
int cmd_sub;
int status_sub;
int safety_sub;
int armed_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
@@ -717,9 +717,9 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- SAFETY --- */
subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
fds[fdsc_count].fd = subs.safety_sub;
/* --- ACTUATOR ARMED --- */
subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
fds[fdsc_count].fd = subs.armed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -896,12 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
/* --- SAFETY- LOG MANAGEMENT --- */
/* --- ARMED- LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety);
orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
if (log_when_armed) {
handle_status(&buf_safety);
handle_status(&buf_armed);
}
handled_topics++;
@@ -912,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
//if (log_when_armed) {
// handle_status(&buf_safety);
// handle_status(&buf_armed);
//}
//handled_topics++;
@@ -944,7 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.flight_mode = 0;
log_msg.body.log_STAT.manual_control_mode = 0;
log_msg.body.log_STAT.manual_sas_mode = 0;
log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */
log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
log_msg.body.log_STAT.battery_current = buf_status.current_battery;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
@@ -1348,10 +1348,10 @@ void handle_command(struct vehicle_command_s *cmd)
}
}
void handle_status(struct actuator_safety_s *safety)
void handle_status(struct actuator_armed_s *armed)
{
if (safety->armed != flag_system_armed) {
flag_system_armed = safety->armed;
if (armed->armed != flag_system_armed) {
flag_system_armed = armed->armed;
if (flag_system_armed) {
sdlog2_start_log();
src/modules/uORB/objects_common.cpp
+6 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
#include "topics/safety.h"
ORB_DEFINE(safety, struct safety_s);
#include "topics/battery_status.h"
ORB_DEFINE(battery_status, struct battery_status_s);
@@ -153,8 +156,8 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
#include "topics/actuator_safety.h"
ORB_DEFINE(actuator_safety, struct actuator_safety_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
/* actuator controls, as set by actuators / mixers after limiting */
#include "topics/actuator_controls_effective.h"
src/modules/uORB/topics/actuator_safety.h → src/modules/uORB/topics/actuator_armed.h
+7 -15
View File
@@ -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
* 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
#define TOPIC_ACTUATOR_SAFETY_H
#ifndef TOPIC_ACTUATOR_ARMED_H
#define TOPIC_ACTUATOR_ARMED_H
#include <stdint.h>
#include "../uORB.h"
/** global 'actuator output is live' control. */
struct actuator_safety_s {
struct actuator_armed_s {
uint64_t timestamp;
bool safety_switch_available; /**< Set to true if a safety switch is connected */
bool safety_off; /**< Set to true if safety is off */
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */
};
ORB_DECLARE(actuator_safety);
ORB_DECLARE(actuator_armed);
#endif
src/modules/uORB/topics/safety.h
+57
View File
@@ -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
src/modules/uORB/topics/vehicle_control_mode.h
+5 -1
View File
@@ -69,9 +69,13 @@ struct vehicle_control_mode_s
bool flag_system_emergency;
bool flag_preflight_calibration;
// XXX needs yet to be set by state machine helper
bool flag_system_hil_enabled;
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
bool flag_auto_enabled;
// XXX shouldn't be necessairy
//bool flag_auto_enabled;
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */