aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
authorJulian Oes <julian@oes.ch>2013-06-14 13:53:26 +0200
committerJulian Oes <julian@oes.ch>2013-06-14 13:53:26 +0200
commit90f5e30f2a177bed2ac08e76699ec3029292d640 (patch)
tree25752b2843f573e3affe42b7e20b8fc06c457290 /src/modules/mavlink
parent236053a600f5708aee0e5849f4fefc2380e7d101 (diff)
downloadpx4-firmware-90f5e30f2a177bed2ac08e76699ec3029292d640.tar.gz
px4-firmware-90f5e30f2a177bed2ac08e76699ec3029292d640.tar.bz2
px4-firmware-90f5e30f2a177bed2ac08e76699ec3029292d640.zip
Introduced new actuator_safety topic
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/orb_listener.c18
-rw-r--r--src/modules/mavlink/orb_topics.h3
2 files changed, 11 insertions, 10 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index d84406e7a..a4a2ca3e5 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos;
struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
-struct actuator_armed_s armed;
+struct actuator_safety_s safety;
struct mavlink_subscriptions mavlink_subs;
@@ -109,7 +109,7 @@ static void l_global_position_setpoint(const struct listener *l);
static void l_local_position_setpoint(const struct listener *l);
static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
-static void l_actuator_armed(const struct listener *l);
+static void l_actuator_safety(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
@@ -133,7 +133,7 @@ static const struct listener listeners[] = {
{l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
{l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
{l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
- {l_actuator_armed, &mavlink_subs.armed_sub, 0},
+ {l_actuator_safety, &mavlink_subs.safety_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
@@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l)
{
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
- orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+ orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
@@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l)
act_outputs.output[7]);
/* only send in HIL mode */
- if (mavlink_hil_enabled && armed.armed) {
+ if (mavlink_hil_enabled && safety.armed) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l)
}
void
-l_actuator_armed(const struct listener *l)
+l_actuator_safety(const struct listener *l)
{
- orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+ orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
}
void
@@ -745,8 +745,8 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
/* --- ACTUATOR ARMED VALUE --- */
- mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */
+ mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
+ orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */
/* --- MAPPED MANUAL CONTROL INPUTS --- */
mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index d61cd43dc..e647b090a 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -58,6 +58,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
@@ -72,7 +73,7 @@ struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
- int armed_sub;
+ int safety_sub;
int actuators_sub;
int local_pos_sub;
int spa_sub;