aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink_main.cpp23
-rw-r--r--src/modules/mavlink/mavlink_main.h5
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp20
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.cpp2
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.h2
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp14
-rw-r--r--src/modules/mavlink/mavlink_stream.cpp4
-rw-r--r--src/modules/mavlink/mavlink_stream.h6
-rw-r--r--src/modules/mavlink/module.mk2
9 files changed, 44 insertions, 34 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 9927b9c3a..6b45736e9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -149,10 +149,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
instance = Mavlink::get_instance(6);
break;
#endif
- }
-
- /* no valid instance, bail */
- if (!instance) {
+ default:
return;
}
@@ -211,9 +208,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
static void usage(void);
Mavlink::Mavlink() :
- next(nullptr),
_device_name(DEFAULT_DEVICE_NAME),
_task_should_exit(false),
+ next(nullptr),
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
@@ -234,7 +231,6 @@ Mavlink::Mavlink() :
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
-
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
@@ -736,9 +732,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
- static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+ char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
- static mavlink_message_t tx_msg;
+ mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
@@ -1543,6 +1539,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
+ uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
@@ -2039,14 +2037,14 @@ Mavlink::task_main(int argc, char *argv[])
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
- warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate);
+ warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
} else {
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
}
} else {
- warnx("stream %s not found", _subscribe_to_stream, _device_name);
+ warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
}
delete _subscribe_to_stream;
@@ -2213,7 +2211,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
- 2048,
+ 1950,
(main_t)&Mavlink::start_helper,
(const char **)argv);
@@ -2252,7 +2250,6 @@ Mavlink::stream(int argc, char *argv[])
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
const char *stream_name = nullptr;
- int ch;
argc -= 2;
argv += 2;
@@ -2289,7 +2286,7 @@ Mavlink::stream(int argc, char *argv[])
i++;
}
- if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
+ if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name);
if (inst != nullptr) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1bf51fd31..25c0da820 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -221,8 +221,6 @@ private:
int _mavlink_fd;
bool _task_running;
- perf_counter_t _loop_perf; /**< loop performance counter */
-
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
@@ -237,7 +235,6 @@ private:
orb_advert_t _mission_pub;
struct mission_s mission;
- uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
@@ -283,7 +280,7 @@ private:
pthread_mutex_t _message_buffer_mutex;
-
+ perf_counter_t _loop_perf; /**< loop performance counter */
/**
* Send one parameter.
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index ec90cf599..70087cf3e 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -125,18 +125,22 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
- } else if (status->main_state == MAIN_STATE_SEATBELT) {
+ } else if (status->main_state == MAIN_STATE_ALTCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
- } else if (status->main_state == MAIN_STATE_EASY) {
+ } else if (status->main_state == MAIN_STATE_POSCTL) {
*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;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
} else if (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 if (status->main_state == MAIN_STATE_ACRO) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
}
} else {
@@ -1139,10 +1143,10 @@ protected:
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
- manual->roll * 1000,
- manual->pitch * 1000,
- manual->yaw * 1000,
- manual->throttle * 1000,
+ manual->x * 1000,
+ manual->y * 1000,
+ manual->z * 1000,
+ manual->r * 1000,
0);
}
}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index d432edd2b..21d5219d3 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription()
free(_data);
}
-const orb_id_t
+orb_id_t
MavlinkOrbSubscription::get_topic()
{
return _topic;
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 5c6543e81..8c09772c8 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -63,7 +63,7 @@ public:
*/
bool is_published();
void *get_data();
- const orb_id_t get_topic();
+ orb_id_t get_topic();
private:
const orb_id_t _topic; /*< topic metadata */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4f83c6b95..33358b7b6 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
}
}
-
+
/* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true);
@@ -438,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
memset(&manual, 0, sizeof(manual));
manual.timestamp = hrt_absolute_time();
- manual.pitch = man.x / 1000.0f;
- manual.roll = man.y / 1000.0f;
- manual.yaw = man.r / 1000.0f;
- manual.throttle = man.z / 1000.0f;
+ manual.x = man.x / 1000.0f;
+ manual.y = man.y / 1000.0f;
+ manual.r = man.r / 1000.0f;
+ manual.z = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context)
rcv->receive_thread(NULL);
delete rcv;
+
+ return nullptr;
}
pthread_t
@@ -949,7 +951,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
- pthread_attr_setstacksize(&receiveloop_attr, 3000);
+ pthread_attr_setstacksize(&receiveloop_attr, 2900);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index bb19d7e33..5ec30bd33 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t)
/* interval expired, send message */
send(t);
_last_sent = (t / _interval) * _interval;
+
+ return 0;
}
+
+ return -1;
}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index def40d9ad..2979d20de 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -63,9 +63,13 @@ public:
MavlinkStream *next;
MavlinkStream();
- ~MavlinkStream();
+ virtual ~MavlinkStream();
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
+
+ /**
+ * @return 0 if updated / sent, -1 if unchanged
+ */
int update(const hrt_abstime t);
virtual MavlinkStream *new_instance() = 0;
virtual void subscribe(Mavlink *mavlink) = 0;
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index dcca11977..f532e26fe 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -48,3 +48,5 @@ SRCS += mavlink_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
+
+MODULE_STACKSIZE = 1024
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-10 00:07:47 +0000