aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c114
-rw-r--r--src/modules/mavlink/orb_listener.c22
-rw-r--r--src/modules/mavlink/orb_topics.h4
3 files changed, 68 insertions, 72 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 5b8345e7e..f6c371c7c 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -187,95 +187,88 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
- /* set mode flags independent of system state */
+ /**
+ * Set mode flags
+ **/
/* HIL */
if (v_status.flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
- /* manual input */
- if (v_status.flag_control_manual_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
- }
-
- /* attitude or rate control */
- if (v_status.flag_control_attitude_enabled ||
- v_status.flag_control_rates_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
- }
-
- /* vector control */
- if (v_status.flag_control_velocity_enabled ||
- v_status.flag_control_position_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
- }
-
/* autonomous mode */
- if (v_status.state_machine == SYSTEM_STATE_AUTO) {
+ if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
}
/* set arming state */
- if (armed.armed) {
+ if (v_status.arming_state == ARMING_STATE_ARMED
+ || v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
- switch (v_status.state_machine) {
- case SYSTEM_STATE_PREFLIGHT:
- if (v_status.flag_preflight_gyro_calibration ||
- v_status.flag_preflight_mag_calibration ||
- v_status.flag_preflight_accel_calibration) {
- *mavlink_state = MAV_STATE_CALIBRATING;
+ if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
+ || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
- } else {
- *mavlink_state = MAV_STATE_UNINIT;
- }
+ *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ }
- break;
+ if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) {
- case SYSTEM_STATE_STANDBY:
- *mavlink_state = MAV_STATE_STANDBY;
- break;
+ *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+ }
- case SYSTEM_STATE_GROUND_READY:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
- case SYSTEM_STATE_MANUAL:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
+ /**
+ * Set mavlink state
+ **/
- case SYSTEM_STATE_STABILIZED:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
+ /* set calibration state */
+ if (v_status.flag_preflight_calibration) {
- case SYSTEM_STATE_AUTO:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
+ *mavlink_state = MAV_STATE_CALIBRATING;
- case SYSTEM_STATE_MISSION_ABORT:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
+ } else if (v_status.flag_system_emergency) {
- case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY;
- break;
- case SYSTEM_STATE_EMCY_CUTOFF:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
+ // XXX difference between active and armed? is AUTO_READY active?
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+ || v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
- case SYSTEM_STATE_GROUND_ERROR:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
+ *mavlink_state = MAV_STATE_ACTIVE;
- case SYSTEM_STATE_REBOOT:
- *mavlink_state = MAV_STATE_POWEROFF;
- break;
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+ || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+ || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
+
+ *mavlink_state = MAV_STATE_STANDBY;
+
+ } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
+
+ *mavlink_state = MAV_STATE_UNINIT;
+ } else {
+
+ warnx("Unknown mavlink state");
+ *mavlink_state = MAV_STATE_CRITICAL;
}
}
@@ -571,6 +564,7 @@ int mavlink_thread_main(int argc, char *argv[])
default:
usage();
+ break;
}
}
@@ -681,7 +675,7 @@ int mavlink_thread_main(int argc, char *argv[])
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */
- mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 0597555ab..a9a611998 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 actuator_controls_effective_s actuators_0;
struct vehicle_attitude_s att;
@@ -111,7 +111,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);
@@ -136,7 +136,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},
@@ -270,7 +270,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);
@@ -285,7 +285,7 @@ l_vehicle_status(const struct listener *l)
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_mode,
- v_status.state_machine,
+ v_status.navigation_state,
mavlink_state);
}
@@ -467,7 +467,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;
@@ -539,9 +539,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
@@ -674,7 +674,7 @@ uorb_receive_thread(void *arg)
/* handle the poll result */
if (poll_ret == 0) {
- mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+ /* silent */
} else if (poll_ret < 0) {
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -760,8 +760,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 73e278dc6..3b968b911 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,10 +55,12 @@
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#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/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
@@ -75,7 +77,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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 07:20:55 +0000