dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: use inherited classes instead of callbacks for mavlink messages formatting, fixes and cleanup

This commit is contained in:
Anton Babushkin 2014-02-26 21:28:35 +04:00
parent 769a2af1f8
commit 7310fd6085
9 changed files with 444 additions and 362 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

202
src/modules/mavlink/mavlink_main.cpp
View File

@ -152,12 +152,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
static void usage(void);
namespace mavlink
{
Mavlink *g_mavlink;
}
Mavlink::Mavlink() :
device_name("/dev/ttyS1"),
_task_should_exit(false),
@ -166,14 +160,13 @@ Mavlink::Mavlink() :
thread_running(false),
_mavlink_task(-1),
_mavlink_incoming_fd(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")),
_mavlink_hil_enabled(false),
_subscriptions(nullptr),
_streams(nullptr),
mission_pub(-1),
mission_pub(-1)
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
wpm = &wpm_s;
fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
@ -204,12 +197,14 @@ Mavlink::~Mavlink()
}
}
void Mavlink::set_mode(enum MAVLINK_MODE mode)
void
Mavlink::set_mode(enum MAVLINK_MODE mode)
{
_mode = mode;
}
int Mavlink::instance_count()
int
Mavlink::instance_count()
{
/* note: a local buffer count will help if this ever is called often */
Mavlink* inst = ::_head;
@ -222,10 +217,11 @@ int Mavlink::instance_count()
return inst_index;
}
Mavlink* Mavlink::new_instance()
Mavlink *
Mavlink::new_instance()
{
Mavlink* inst = new Mavlink();
Mavlink* next = ::_head;
Mavlink *inst = new Mavlink();
Mavlink *next = ::_head;
/* create the first instance at _head */
if (::_head == nullptr) {
@ -241,9 +237,10 @@ Mavlink* Mavlink::new_instance()
return inst;
}
Mavlink* Mavlink::get_instance(unsigned instance)
Mavlink *
Mavlink::get_instance(unsigned instance)
{
Mavlink* inst = ::_head;
Mavlink *inst = ::_head;
unsigned inst_index = 0;
while (inst->_next != nullptr && inst_index < instance) {
inst = inst->_next;
@ -257,7 +254,8 @@ Mavlink* Mavlink::get_instance(unsigned instance)
return inst;
}
int Mavlink::destroy_all_instances()
int
Mavlink::destroy_all_instances()
{
/* start deleting from the end */
Mavlink *inst_to_del = nullptr;
@ -294,7 +292,8 @@ int Mavlink::destroy_all_instances()
return OK;
}
bool Mavlink::instance_exists(const char *device_name, Mavlink *self)
bool
Mavlink::instance_exists(const char *device_name, Mavlink *self)
{
Mavlink* inst = ::_head;
while (inst != nullptr) {
@ -307,7 +306,8 @@ bool Mavlink::instance_exists(const char *device_name, Mavlink *self)
return false;
}
int Mavlink::get_uart_fd(unsigned index)
int
Mavlink::get_uart_fd(unsigned index)
{
Mavlink* inst = get_instance(index);
if (inst)
@ -316,14 +316,16 @@ int Mavlink::get_uart_fd(unsigned index)
return -1;
}
int Mavlink::get_uart_fd()
int
Mavlink::get_uart_fd()
{
return _uart;
}
int Mavlink::get_channel()
mavlink_channel_t
Mavlink::get_channel()
{
return (int)_chan;
return _chan;
}
/****************************************************************************
@ -1364,7 +1366,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata
MavlinkOrbSubscription *sub;
LL_FOREACH(_subscriptions, sub) {
if (sub->topic == topic) {
if (sub->get_topic() == topic) {
/* already subscribed */
return sub;
}
@ -1379,26 +1381,19 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata
}
int
Mavlink::add_stream(const char *stream_name, const unsigned int interval)
Mavlink::add_stream(const char *stream_name, const float rate)
{
uintptr_t arg = 0;
unsigned int i = 0;
/* search for message with specified name */
while (msgs_list[i].name != nullptr) {
if (strcmp(stream_name, msgs_list[i].name) == 0) {
/* count topics, array is nullptr-terminated */
unsigned int topics_n;
for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) {
if (msgs_list[i].topics[topics_n] == nullptr) {
break;
}
}
MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval);
/* search for stream with specified name */
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create stream copy for each mavlink instance */
MavlinkStream *stream = streams_list[i]->new_instance();
stream->set_channel(get_channel());
stream->set_interval(1000.0f / rate);
stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
}
i++;
}
return ERROR;
}
@ -1407,7 +1402,7 @@ int
Mavlink::task_main(int argc, char *argv[])
{
/* inform about start */
warnx("Initializing..");
warnx("start");
fflush(stdout);
/* initialize mavlink text message buffering */
@ -1463,9 +1458,6 @@ Mavlink::task_main(int argc, char *argv[])
bool usb_uart;
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
switch (_mode) {
case MODE_TX_HEARTBEAT_ONLY:
@ -1510,13 +1502,13 @@ Mavlink::task_main(int argc, char *argv[])
err(1, "could not open %s", device_name);
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
if (instance_count() == 1) {
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
}
/* initialize logging device */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* Initialize system properties */
mavlink_update_system();
@ -1537,8 +1529,10 @@ Mavlink::task_main(int argc, char *argv[])
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s));
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s));
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data;
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
warnx("started");
mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* add default streams, intervals depend on baud rate */
// if (_baudrate >= 230400) {
@ -1546,15 +1540,15 @@ Mavlink::task_main(int argc, char *argv[])
// } else if (_baudrate >= 57600) {
// }
add_stream("HEARTBEAT", 1000);
add_stream("SYS_STATUS", 1000);
add_stream("HIGHRES_IMU", 300);
add_stream("RAW_IMU", 300);
add_stream("ATTITUDE", 200);
add_stream("NAMED_VALUE_FLOAT", 200);
add_stream("SERVO_OUTPUT_RAW", 500);
add_stream("GPS_RAW_INT", 500);
add_stream("MANUAL_CONTROL", 500);
add_stream("HEARTBEAT", 1.0f);
add_stream("SYS_STATUS", 1.0f);
add_stream("HIGHRES_IMU", 20.0f);
// add_stream("RAW_IMU", 10.0f);
add_stream("ATTITUDE", 20.0f);
// add_stream("NAMED_VALUE_FLOAT", 5.0f);
// add_stream("SERVO_OUTPUT_RAW", 2.0f);
// add_stream("GPS_RAW_INT", 2.0f);
// add_stream("MANUAL_CONTROL", 2.0f);
while (!_task_should_exit) {
/* main loop */
@ -1604,31 +1598,24 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
}
mavlink_waypoint_eventloop(hrt_absolute_time());
/* check if waypoint has been reached against the last positions */
mavlink_waypoint_eventloop(hrt_absolute_time());
if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) {
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(hrt_absolute_time());
mavlink_waypoint_eventloop(hrt_absolute_time());
if (_baudrate > 57600) {
mavlink_pm_queued_send();
}
/* send one string at 10 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
/* send one string at 20 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
}
}
}
}
perf_end(_loop_perf);
@ -1636,7 +1623,6 @@ Mavlink::task_main(int argc, char *argv[])
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
pthread_join(uorb_receive_thread, NULL);
/* Reset the UART flags to original state */
tcsetattr(_uart, TCSANOW, &uart_config_original);
@ -1654,15 +1640,50 @@ Mavlink::task_main(int argc, char *argv[])
int Mavlink::start_helper(int argc, char *argv[])
{
// Create the instance in task context
/* create the instance in task context */
Mavlink *instance = Mavlink::new_instance();
// This will actually only return once MAVLink exits
/* this will actually only return once MAVLink exits */
return instance->task_main(argc, argv);
}
int
Mavlink::start()
Mavlink::start(int argc, char *argv[])
{
// Instantiate thread
char buf[32];
sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
(main_t)&Mavlink::start_helper,
(const char **)argv);
// while (!this->is_running()) {
// usleep(200);
// }
// if (mavlink->_mavlink_task < 0) {
// warn("task start failed");
// return -errno;
// }
// if (mavlink::g_mavlink != nullptr) {
// errx(1, "already running");
// }
// mavlink::g_mavlink = new Mavlink;
// if (mavlink::g_mavlink == nullptr) {
// errx(1, "alloc failed");
// }
// if (OK != mavlink::g_mavlink->start()) {
// delete mavlink::g_mavlink;
// mavlink::g_mavlink = nullptr;
// err(1, "start failed");
// }
return OK;
}
@ -1685,44 +1706,7 @@ int mavlink_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "start")) {
// Instantiate thread
char buf[32];
sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&Mavlink::start_helper,
(const char **)argv);
// while (!this->is_running()) {
// usleep(200);
// }
// if (mavlink->_mavlink_task < 0) {
// warn("task start failed");
// return -errno;
// }
// if (mavlink::g_mavlink != nullptr) {
// errx(1, "already running");
// }
// mavlink::g_mavlink = new Mavlink;
// if (mavlink::g_mavlink == nullptr) {
// errx(1, "alloc failed");
// }
// if (OK != mavlink::g_mavlink->start()) {
// delete mavlink::g_mavlink;
// mavlink::g_mavlink = nullptr;
// err(1, "start failed");
// }
return 0;
return Mavlink::start(argc, argv);
} else if (!strcmp(argv[1], "stop")) {
warnx("mavlink stop is deprecated, use stop-all instead");

16
src/modules/mavlink/mavlink_main.h
View File

@ -142,7 +142,7 @@ public:
*
* @return OK on success.
*/
static int start();
static int start(int argc, char *argv[]);
/**
* Display the mavlink status.
@ -163,8 +163,6 @@ public:
int get_uart_fd();
int get_channel();
const char *device_name;
enum MAVLINK_MODE {
@ -205,16 +203,11 @@ public:
MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
mavlink_channel_t get_chan() { return _chan; }
mavlink_channel_t get_channel();
bool _task_should_exit; /**< if true, mavlink task should exit */
protected:
/**
* Pointer to the default cdev file operations table; useful for
* registering clone devices etc.
*/
Mavlink* _next;
private:
@ -234,7 +227,7 @@ private:
orb_advert_t mission_pub;
struct mission_s mission;
uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)];
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
@ -247,7 +240,6 @@ private:
unsigned int total_counter;
pthread_t receive_thread;
pthread_t uorb_receive_thread;
/* Allocate storage space for waypoints */
mavlink_wpm_storage wpm_s;
@ -326,7 +318,7 @@ private:
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
int add_stream(const char *stream_name, const unsigned int interval);
int add_stream(const char *stream_name, const float rate);
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);

440
src/modules/mavlink/mavlink_messages.cpp
View File

@ -12,195 +12,289 @@
#include "mavlink_messages.h"
struct msgs_list_s msgs_list[] = {
{
.name = "HEARTBEAT",
.callback = msg_heartbeat,
.topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr },
.sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) }
},
{
.name = "SYS_STATUS",
.callback = msg_sys_status,
.topics = { ORB_ID(vehicle_status), nullptr },
.sizes = { sizeof(struct vehicle_status_s) }
},
{ .name = nullptr }
};
void
msg_heartbeat(const MavlinkStream *stream)
{
struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data;
struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data;
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
/* HIL */
if (status->hil_state == HIL_STATE_ON) {
mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
class MavlinkStreamHeartbeat : public MavlinkStream {
public:
const char *get_name()
{
return "HEARTBEAT";
}
/* arming state */
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
MavlinkStream *new_instance()
{
return new MavlinkStreamHeartbeat();
}
/* main state */
mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
/* use main state when navigator is not active */
if (status->main_state == MAIN_STATE_MANUAL) {
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) {
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 (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 (status->main_state == MAIN_STATE_AUTO) {
private:
MavlinkOrbSubscription *status_sub;
MavlinkOrbSubscription *pos_sp_triplet_sub;
struct vehicle_status_s *status;
struct position_setpoint_triplet_s *pos_sp_triplet;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
status = (struct vehicle_status_s *)status_sub->get_data();
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data();
}
void send(const hrt_abstime t) {
status_sub->update(t);
pos_sp_triplet_sub->update(t);
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
/* HIL */
if (status->hil_state == HIL_STATE_ON) {
mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* arming state */
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
/* main state */
mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
/* use main state when navigator is not active */
if (status->main_state == MAIN_STATE_MANUAL) {
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) {
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 (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 (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;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} 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;
}
}
} 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 (pos_sp_triplet->nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} 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;
mavlink_custom_mode = custom_mode.data;
/* set system state */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
mavlink_state = MAV_STATE_UNINIT;
} else if (status->arming_state == ARMING_STATE_ARMED) {
mavlink_state = MAV_STATE_ACTIVE;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
mavlink_state = MAV_STATE_CRITICAL;
} else if (status->arming_state == ARMING_STATE_STANDBY) {
mavlink_state = MAV_STATE_STANDBY;
} else if (status->arming_state == ARMING_STATE_REBOOT) {
mavlink_state = MAV_STATE_POWEROFF;
} else {
mavlink_state = MAV_STATE_CRITICAL;
/* set system state */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
mavlink_state = MAV_STATE_UNINIT;
} else if (status->arming_state == ARMING_STATE_ARMED) {
mavlink_state = MAV_STATE_ACTIVE;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
mavlink_state = MAV_STATE_CRITICAL;
} else if (status->arming_state == ARMING_STATE_STANDBY) {
mavlink_state = MAV_STATE_STANDBY;
} else if (status->arming_state == ARMING_STATE_REBOOT) {
mavlink_state = MAV_STATE_POWEROFF;
} else {
mavlink_state = MAV_STATE_CRITICAL;
}
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
};
class MavlinkStreamSysStatus : public MavlinkStream {
public:
const char *get_name()
{
return "SYS_STATUS";
}
/* send heartbeat */
mavlink_msg_heartbeat_send(stream->mavlink->get_chan(),
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
MavlinkStream *new_instance()
{
return new MavlinkStreamSysStatus();
}
private:
MavlinkOrbSubscription *status_sub;
struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t) {
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
};
class MavlinkStreamHighresIMU : public MavlinkStream {
public:
const char *get_name()
{
return "HIGHRES_IMU";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamHighresIMU();
}
private:
MavlinkOrbSubscription *sensor_sub;
struct sensor_combined_s *sensor;
uint32_t accel_counter = 0;
uint32_t gyro_counter = 0;
uint32_t mag_counter = 0;
uint32_t baro_counter = 0;
protected:
void subscribe(Mavlink *mavlink)
{
sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s));
sensor = (struct sensor_combined_s *)sensor_sub->get_data();
}
void send(const hrt_abstime t) {
sensor_sub->update(t);
uint16_t fields_updated = 0;
if (accel_counter != sensor->accelerometer_counter) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
accel_counter = sensor->accelerometer_counter;
}
if (gyro_counter != sensor->gyro_counter) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
gyro_counter = sensor->gyro_counter;
}
if (mag_counter != sensor->magnetometer_counter) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
mag_counter = sensor->magnetometer_counter;
}
if (baro_counter != sensor->baro_counter) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
baro_counter = sensor->baro_counter;
}
mavlink_msg_highres_imu_send(_channel,
sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
}
};
class MavlinkStreamAttitude : public MavlinkStream {
public:
const char *get_name()
{
return "ATTITUDE";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamAttitude();
}
private:
MavlinkOrbSubscription *att_sub;
struct vehicle_attitude_s *att;
protected:
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t) {
att_sub->update(t);
mavlink_msg_attitude_send(_channel,
att->timestamp / 1000,
att->roll, att->pitch, att->yaw,
att->rollspeed, att->pitchspeed, att->yawspeed);
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
new MavlinkStreamHighresIMU(),
new MavlinkStreamAttitude(),
nullptr
};
void
msg_sys_status(const MavlinkStream *stream)
{
struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data;
mavlink_msg_sys_status_send(stream->mavlink->get_chan(),
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
void
msg_highres_imu(const MavlinkStream *stream)
{
struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data;
uint16_t fields_updated = 0;
if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
}
//void
//MavlinkOrbListener::l_sensor_combined(const struct listener *l)
//{
// struct sensor_combined_s raw;
//
// /* copy sensors raw data into local buffer */
// orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
//
// /* mark individual fields as _mavlink->get_chan()ged */
// uint16_t fields_updated = 0;
//
// // if (accel_counter != raw.accelerometer_counter) {
// // /* mark first three dimensions as _mavlink->get_chan()ged */
// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
// // accel_counter = raw.accelerometer_counter;
// // }
//
// // if (gyro_counter != raw.gyro_counter) {
// // /* mark second group dimensions as _mavlink->get_chan()ged */
// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
// // gyro_counter = raw.gyro_counter;
// // }
//
// // if (mag_counter != raw.magnetometer_counter) {
// // /* mark third group dimensions as _mavlink->get_chan()ged */
// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
// // mag_counter = raw.magnetometer_counter;
// // }
//
// // if (baro_counter != raw.baro_counter) {
// // /* mark last group dimensions as _mavlink->get_chan()ged */
// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
// // baro_counter = raw.baro_counter;
// // }
//
// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp,
// raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
// raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
// raw.gyro_rad_s[1], raw.gyro_rad_s[2],
// raw.magnetometer_ga[0],
// raw.magnetometer_ga[1], raw.magnetometer_ga[2],
// raw.baro_pres_mbar, raw.differential_pressure_pa,
// raw.baro_alt_meter, raw.baro_temp_celcius,
// fields_updated);
//
// l->listener->sensors_raw_counter++;
//}
//
//void
//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
//{

15
src/modules/mavlink/mavlink_messages.h
View File

@ -10,19 +10,6 @@
#include "mavlink_stream.h"
#define MAX_TOPICS_PER_MAVLINK_STREAM 4
struct msgs_list_s {
char *name;
void (*callback)(const MavlinkStream *);
const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1];
size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1];
};
extern struct msgs_list_s msgs_list[];
static void msg_heartbeat(const MavlinkStream *stream);
static void msg_sys_status(const MavlinkStream *stream);
static void msg_highres_imu(const MavlinkStream *stream);
extern MavlinkStream *streams_list[];
#endif /* MAVLINK_MESSAGES_H_ */

36
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -13,28 +13,40 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size)
MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
{
this->topic = topic;
this->data = malloc(size);
memset(this->data, 0, size);
this->fd = orb_subscribe(topic);
this->last_update = 0;
_data = malloc(size);
memset(_data, 0, size);
_fd = orb_subscribe(_topic);
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
close(fd);
free(data);
close(_fd);
free(_data);
}
bool MavlinkOrbSubscription::update(const hrt_abstime t)
const struct orb_metadata *
MavlinkOrbSubscription::get_topic()
{
if (last_update != t) {
return _topic;
}
void *
MavlinkOrbSubscription::get_data()
{
return _data;
}
bool
MavlinkOrbSubscription::update(const hrt_abstime t)
{
if (_last_check != t) {
_last_check = t;
bool updated;
orb_check(fd, &updated);
orb_check(_fd, &updated);
if (updated) {
orb_copy(topic, fd, data);
orb_copy(_topic, _fd, _data);
return true;
}
}

17
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -11,18 +11,23 @@
#include <systemlib/uthash/utlist.h>
#include <drivers/drv_hrt.h>
class MavlinkOrbSubscription {
public:
MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size);
MavlinkOrbSubscription *next;
MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size);
~MavlinkOrbSubscription();
bool update(const hrt_abstime t);
void *get_data();
const struct orb_metadata *get_topic();
const struct orb_metadata *topic;
int fd;
void *data;
hrt_abstime last_update;
MavlinkOrbSubscription *next;
private:
const struct orb_metadata *_topic;
int _fd;
void *_data;
hrt_abstime _last_check;
};

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -818,7 +818,7 @@ MavlinkReceiver::receive_thread(void *arg)
/* if read failed, this loop won't execute */
for (ssize_t i = 0; i < nread; i++) {
if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) {
if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
/* handle generic messages and commands */
handle_message(&msg);

49
src/modules/mavlink/mavlink_stream.cpp
View File

@ -10,39 +10,42 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval)
MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
{
this->callback = callback;
this->arg = arg;
this->interval = interval * 1000;
this->mavlink = mavlink;
this->subscriptions_n = subs_n;
this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *));
for (int i = 0; i < subs_n; i++) {
this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]);
}
}
MavlinkStream::~MavlinkStream()
{
free(subscriptions);
}
/**
* Update mavlink stream, i.e. update subscriptions and send message if necessary
* Set messages interval in ms
*/
int MavlinkStream::update(const hrt_abstime t)
void
MavlinkStream::set_interval(const unsigned int interval)
{
uint64_t dt = t - last_sent;
if (dt > 0 && dt >= interval) {
/* interval expired, update all subscriptions */
for (unsigned int i = 0; i < subscriptions_n; i++) {
subscriptions[i]->update(t);
}
_interval = interval * 1000;
}
/* format and send mavlink message */
callback(this);
last_sent = t;
/**
* Set mavlink channel
*/
void
MavlinkStream::set_channel(mavlink_channel_t channel)
{
_channel = channel;
}
/**
* Update subscriptions and send message if necessary
*/
int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
/* interval expired, send message */
send(t);
_last_sent = (t / _interval) * _interval;
}
}

29
src/modules/mavlink/mavlink_stream.h
View File

@ -15,22 +15,27 @@ class MavlinkStream;
#include "mavlink_main.h"
class MavlinkOrbSubscription;
class MavlinkStream {
public:
void (*callback)(const MavlinkStream *);
uintptr_t arg;
unsigned int subscriptions_n;
MavlinkOrbSubscription **subscriptions;
hrt_abstime last_sent;
unsigned int interval;
MavlinkStream *next;
Mavlink *mavlink;
private:
hrt_abstime _last_sent;
MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval);
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
public:
MavlinkStream *next;
MavlinkStream();
~MavlinkStream();
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
int update(const hrt_abstime t);
virtual MavlinkStream *new_instance() = 0;
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() = 0;
};