aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c2
-rw-r--r--src/modules/mavlink/mavlink_main.cpp42
-rw-r--r--src/modules/mavlink/mavlink_main.h8
-rw-r--r--src/modules/mavlink/mavlink_orb_listener.cpp32
-rw-r--r--src/modules/mavlink/mavlink_orb_listener.h3
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp4
-rw-r--r--src/modules/mavlink/mavlink_receiver.h2
7 files changed, 39 insertions, 54 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index b143e62f0..2c7ca7525 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -56,7 +56,7 @@
// #include <stdlib.h>
// #include <poll.h>
-//
+//
// #include <systemlib/systemlib.h>
// #include <systemlib/err.h>
// #include <mavlink/mavlink_log.h>
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 4f4d94947..2585593ea 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -61,7 +61,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <systemlib/param/param.h>
@@ -502,28 +502,36 @@ Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_bas
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
- if (v_status.main_state == MAIN_STATE_MANUAL) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
- } else if (v_status.main_state == MAIN_STATE_SEATBELT) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
- } else if (v_status.main_state == MAIN_STATE_EASY) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
- } else if (v_status.main_state == MAIN_STATE_AUTO) {
+ if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
+ /* use main state when navigator is not active */
+ if (v_status.main_state == MAIN_STATE_MANUAL) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+ } else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+ } else if (v_status.main_state == MAIN_STATE_EASY) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+ } else if (v_status.main_state == MAIN_STATE_AUTO) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+ }
+ } else {
+ /* use navigation state when navigator is active */
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
- if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
- } else if (control_mode.nav_state == NAV_STATE_READY) {
+ if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
- } else if (control_mode.nav_state == NAV_STATE_LOITER) {
+ } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
- } else if (control_mode.nav_state == NAV_STATE_MISSION) {
+ } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
- } else if (control_mode.nav_state == NAV_STATE_RTL) {
+ } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+ } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
}
*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 9b289c965..ce2d2b34a 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -57,7 +57,7 @@
#include <uORB/topics/vehicle_vicon_position.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/position_setpoint_triplet.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -233,7 +233,7 @@ public:
int get_sub;
int airspeed_sub;
int navigation_capabilities_sub;
- int control_mode_sub;
+ int position_setpoint_triplet_sub;
int rc_sub;
int status_sub;
int home_sub;
@@ -252,12 +252,12 @@ public:
struct navigation_capabilities_s nav_cap;
/** Vehicle status */
struct vehicle_status_s v_status;
- /** Vehicle control mode */
- struct vehicle_control_mode_s control_mode;
/** RC channels */
struct rc_channels_s rc;
/** Actuator armed state */
struct actuator_armed_s armed;
+ /** Position setpoint triplet */
+ struct position_setpoint_triplet_s pos_sp_triplet;
protected:
/**
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index 418e1c121..cd9408f2f 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -94,7 +94,6 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0);
add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0);
- add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0);
add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0);
add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0);
add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0);
@@ -277,6 +276,7 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
/* immediately communicate state _mavlink->get_chan()ges back to user */
orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status));
orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
+ orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet));
/* enable or disable HIL */
if (l->listener->v_status.hil_state == HIL_STATE_ON)
@@ -317,10 +317,10 @@ MavlinkOrbListener::l_input_rc(const struct listener *l)
const unsigned port_width = 8;
- for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
+ for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(),
- l->listener->rc_raw.timestamp / 1000,
+ l->listener->rc_raw.timestamp_publication / 1000,
i,
(l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
(l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
@@ -644,26 +644,6 @@ MavlinkOrbListener::l_nav_cap(const struct listener *l)
}
-void
-MavlinkOrbListener::l_control_mode(const struct listener *l)
-{
- orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode);
-
- /* translate the current syste state to mavlink state and mode */
- uint8_t mavlink_state = 0;
- uint8_t mavlink_base_mode = 0;
- uint32_t mavlink_custom_mode = 0;
- l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
- /* send heartbeat */
- mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
- mavlink_system.type,
- MAV_AUTOPILOT_PX4,
- mavlink_base_mode,
- mavlink_custom_mode,
- mavlink_state);
-}
-
void *
MavlinkOrbListener::uorb_receive_thread(void *arg)
{
@@ -759,9 +739,9 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */
- /* --- CONTROL MODE --- */
- mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
- orb_set_interval(mavlink->get_subs()->control_mode_sub, 300); /* max 3.33 Hz updates */
+ /* --- POSITION SETPOINT TRIPLET --- */
+ mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+ orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0); /* not polled, don't limit */
/* --- RC CHANNELS VALUE --- */
mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels));
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index 349a11f2e..560b47423 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -57,7 +57,6 @@
#include <uORB/topics/vehicle_vicon_position.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>
@@ -158,13 +157,11 @@ private:
static void l_home(const struct listener *l);
static void l_airspeed(const struct listener *l);
static void l_nav_cap(const struct listener *l);
- static void l_control_mode(const struct listener *l);
struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
- struct vehicle_control_mode_s control_mode;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 94d97fc12..89dcb1d7b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -330,7 +330,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
- if (telemetry_status_pub == 0) {
+ if (telemetry_status_pub <= 0) {
telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
@@ -613,7 +613,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
hil_global_pos.timestamp = timestamp;
- hil_global_pos.valid = true;
+ hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index be32ce0f7..d8d3b5452 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -57,7 +57,6 @@
#include <uORB/topics/vehicle_vicon_position.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>
@@ -122,6 +121,7 @@ private:
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
struct battery_status_s hil_battery_status;
+ struct position_setpoint_triplet_s pos_sp_triplet;
orb_advert_t pub_hil_global_pos;
orb_advert_t pub_hil_local_pos;
orb_advert_t pub_hil_attitude;
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-01 05:33:39 +0000