mavlink: major rewrite, prepare for dynamic mavlink streams configuration, WIP

This commit is contained in:
Anton Babushkin 2014-02-24 23:46:58 +04:00
parent b17cdb12b0
commit d8fdade6ab
7 changed files with 718 additions and 732 deletions

File diff suppressed because it is too large Load Diff

View File

@ -40,9 +40,12 @@
*/ */
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <pthread.h>
#pragma once #pragma once
#include <drivers/drv_rc_input.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
@ -68,11 +71,14 @@
#include <uORB/topics/airspeed.h> #include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/position_setpoint_triplet.h>
#include <drivers/drv_rc_input.h>
#include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include "mavlink_orb_subscription.h"
#include "mavlink_stream.h"
class Mavlink; class Mavlink;
class MavlinkStream;
class MavlinkOrbListener class MavlinkOrbListener
{ {
@ -83,22 +89,14 @@ public:
MavlinkOrbListener(Mavlink* parent); MavlinkOrbListener(Mavlink* parent);
/** /**
* Destructor, also kills the mavlinks task. * Destructor, closes all subscriptions.
*/ */
~MavlinkOrbListener(); ~MavlinkOrbListener();
static pthread_t uorb_receive_start(Mavlink *mavlink); static pthread_t uorb_receive_start(Mavlink *mavlink);
struct listener { MavlinkOrbSubscription *add_subscription(const struct orb_metadata *meta, size_t size, const MavlinkStream *stream, const unsigned int interval);
void (*callback)(const struct listener *l); void add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval);
int *subp;
uintptr_t arg;
struct listener *next;
Mavlink *mavlink;
MavlinkOrbListener* listener;
};
void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg);
static void * uorb_start_helper(void *context); static void * uorb_start_helper(void *context);
private: private:
@ -107,57 +105,34 @@ private:
Mavlink* _mavlink; Mavlink* _mavlink;
struct listener *_listeners; MavlinkOrbSubscription *_subscriptions;
unsigned _n_listeners; static const unsigned _max_subscriptions = 32;
static const unsigned _max_listeners = 32; MavlinkStream *_streams;
void *uorb_receive_thread(void *arg); void *uorb_receive_thread(void *arg);
static void l_sensor_combined(const struct listener *l); static void msg_heartbeat(const MavlinkStream *stream);
static void l_vehicle_attitude(const struct listener *l);
static void l_vehicle_gps_position(const struct listener *l);
static void l_vehicle_status(const struct listener *l);
static void l_rc_channels(const struct listener *l);
static void l_input_rc(const struct listener *l);
static void l_global_position(const struct listener *l);
static void l_local_position(const struct listener *l);
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_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);
static void l_optical_flow(const struct listener *l);
static void l_vehicle_rates_setpoint(const struct listener *l);
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);
struct vehicle_global_position_s global_pos; // static void l_sensor_combined(const struct listener *l);
struct vehicle_local_position_s local_pos; // static void l_vehicle_attitude(const struct listener *l);
struct navigation_capabilities_s nav_cap; // static void l_vehicle_gps_position(const struct listener *l);
struct vehicle_status_s v_status; // static void l_vehicle_status(const struct listener *l);
struct rc_channels_s rc; // static void l_rc_channels(const struct listener *l);
struct rc_input_values rc_raw; // static void l_input_rc(const struct listener *l);
struct actuator_armed_s armed; // static void l_global_position(const struct listener *l);
struct actuator_controls_s actuators_0; // static void l_local_position(const struct listener *l);
struct vehicle_attitude_s att; // static void l_global_position_setpoint(const struct listener *l);
struct airspeed_s airspeed; // static void l_local_position_setpoint(const struct listener *l);
struct home_position_s home; // static void l_attitude_setpoint(const struct listener *l);
// static void l_actuator_outputs(const struct listener *l);
unsigned int sensors_raw_counter; // static void l_actuator_armed(const struct listener *l);
unsigned int attitude_counter; // static void l_manual_control_setpoint(const struct listener *l);
unsigned int gps_counter; // static void l_vehicle_attitude_controls(const struct listener *l);
// static void l_debug_key_value(const struct listener *l);
/* // static void l_optical_flow(const struct listener *l);
* Last sensor loop time // static void l_vehicle_rates_setpoint(const struct listener *l);
* some outputs are better timestamped // static void l_home(const struct listener *l);
* with this "global" reference. // static void l_airspeed(const struct listener *l);
*/ // static void l_nav_cap(const struct listener *l);
uint64_t last_sensor_timestamp;
hrt_abstime last_sent_vfr;
}; };

View File

@ -0,0 +1,47 @@
/*
* mavlink_orb_subscription.cpp
*
* Created on: 23.02.2014
* Author: ton
*/
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <uORB/uORB.h>
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size)
{
this->meta = meta;
this->data = malloc(size);
memset(this->data, 0, size);
this->fd = orb_subscribe(meta);
this->last_update = 0;
this->interval = 0;
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
close(fd);
free(data);
}
int MavlinkOrbSubscription::set_interval(const unsigned int interval)
{
this->interval = interval;
return orb_set_interval(fd, interval);
}
int MavlinkOrbSubscription::update(const hrt_abstime t)
{
if (last_update != t) {
bool updated;
orb_check(fd, &updated);
if (updated)
return orb_copy(meta, fd, &data);
}
return OK;
}

View File

@ -0,0 +1,31 @@
/*
* mavlink_orb_subscription.h
*
* Created on: 23.02.2014
* Author: ton
*/
#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
#define MAVLINK_ORB_SUBSCRIPTION_H_
#include <systemlib/uthash/utlist.h>
#include <drivers/drv_hrt.h>
class MavlinkOrbSubscription {
public:
MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size);
~MavlinkOrbSubscription();
int set_interval(const unsigned int interval);
int update(const hrt_abstime t);
const struct orb_metadata *meta;
int fd;
void *data;
hrt_abstime last_update;
unsigned int interval;
MavlinkOrbSubscription *next;
};
#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */

View File

@ -0,0 +1,48 @@
/*
* mavlink_stream.cpp
*
* Created on: 24.02.2014
* Author: ton
*/
#include <stdlib.h>
#include "mavlink_orb_listener.h"
MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval)
{
this->callback = callback;
this->arg = arg;
this->interval = interval * 1000;
this->mavlink = mavlink;
this->listener = listener;
this->subscriptions_n = subs_n;
this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *));
for (int i = 0; i < subs_n; i++) {
this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval);
}
}
MavlinkStream::~MavlinkStream()
{
free(subscriptions);
}
/**
* Update mavlink stream, i.e. 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, update all subscriptions */
for (unsigned int i = 0; i < subscriptions_n; i++) {
subscriptions[i]->update(t);
}
/* format and send mavlink message */
callback(this);
last_sent = t;
}
}

View File

@ -0,0 +1,33 @@
/*
* mavlink_stream.h
*
* Created on: 24.02.2014
* Author: ton
*/
#ifndef MAVLINK_STREAM_H_
#define MAVLINK_STREAM_H_
class Mavlink;
class MavlinkOrbListener;
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;
MavlinkOrbListener* listener;
MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval);
~MavlinkStream();
int update(const hrt_abstime t);
};
#endif /* MAVLINK_STREAM_H_ */

View File

@ -39,6 +39,8 @@ MODULE_COMMAND = mavlink
SRCS += mavlink_main.cpp \ SRCS += mavlink_main.cpp \
mavlink.c \ mavlink.c \
mavlink_receiver.cpp \ mavlink_receiver.cpp \
mavlink_orb_listener.cpp mavlink_orb_listener.cpp \
mavlink_orb_subscription.cpp \
mavlink_stream.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink