aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c125
-rw-r--r--src/modules/mavlink/orb_listener.c9
-rw-r--r--src/modules/mavlink/orb_topics.h5
3 files changed, 71 insertions, 68 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561..db8ee9067 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -186,95 +186,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) {
+ if (v_status.hil_state == HIL_STATE_ON) {
*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.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.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;
}
}
@@ -568,6 +561,7 @@ int mavlink_thread_main(int argc, char *argv[])
default:
usage();
+ break;
}
}
@@ -678,10 +672,13 @@ 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);
+ if (v_status.hil_state == HIL_STATE_ON)
+ set_hil_on_off(true);
+ else if (v_status.hil_state == HIL_STATE_OFF)
+ set_hil_on_off(false);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
@@ -689,8 +686,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
- v_status.voltage_battery * 1000.0f,
- v_status.current_battery * 1000.0f,
+ v_status.battery_voltage * 1000.0f,
+ v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..d088d421e 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -272,7 +272,10 @@ l_vehicle_status(const struct listener *l)
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
- set_hil_on_off(v_status.flag_hil_enabled);
+ if (v_status.hil_state == HIL_STATE_ON)
+ set_hil_on_off(true);
+ else if (v_status.hil_state == HIL_STATE_OFF)
+ set_hil_on_off(false);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -284,7 +287,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);
}
@@ -673,7 +676,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");
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc6..506f73105 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_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,9 @@ 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 armed_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-28 08:04:32 +0000