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

mavlink: tix mission manager to use MavlinkStream API

This commit is contained in:
Anton Babushkin 2014-07-24 19:31:25 +02:00
parent 897984c4f4
commit b31ddc193c
5 changed files with 97 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -131,8 +131,6 @@ Mavlink::Mavlink() :
_streams(nullptr),
_mission_manager(nullptr),
_parameters_manager(nullptr),
_mission_pub(-1),
_mission_result_sub(-1),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
@ -1255,12 +1253,6 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
/* create mission manager */
_mission_manager = new MavlinkMissionManager(this);
_mission_manager->set_verbose(_verbose);
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
@ -1287,6 +1279,14 @@ Mavlink::task_main(int argc, char *argv[])
_parameters_manager->set_interval(interval_from_rate(30.0f));
LL_APPEND(_streams, _parameters_manager);
/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
* remote requests rate. Rate specified here controls how much bandwidth we will reserve for
* mission messages. */
_mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this);
_mission_manager->set_interval(interval_from_rate(10.0f));
_mission_manager->set_verbose(_verbose);
LL_APPEND(_streams, _mission_manager);
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
@ -1315,12 +1315,8 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
float base_rate_mult = _datarate / 1000.0f;
MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult;
_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
@ -1371,11 +1367,6 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
if (fast_rate_limiter.check(t)) {
// TODO missions
//_mission_manager->eventloop();
}
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {

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

@ -289,8 +289,6 @@ private:
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
orb_advert_t _mission_pub;
int _mission_result_sub;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;

25
src/modules/mavlink/mavlink_mission.cpp
View File

@ -59,8 +59,7 @@
static const int ERROR = -1;
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_mavlink(mavlink),
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
_time_last_recv(0),
_time_last_sent(0),
@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_offboard_mission_sub(-1),
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
_slow_rate_limiter(_interval / 10.0f),
_verbose(false),
_channel(mavlink->get_channel()),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
close(_mission_result_sub);
}
unsigned
MavlinkMissionManager::get_size()
{
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
void
MavlinkMissionManager::init_offboard_mission()
{
@ -275,9 +287,10 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
void
MavlinkMissionManager::eventloop()
MavlinkMissionManager::send(const hrt_abstime now)
{
hrt_abstime now = hrt_absolute_time();
/* update interval for slow rate limiter */
_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
bool updated = false;
orb_check(_mission_result_sub, &updated);

118
src/modules/mavlink/mavlink_mission.h
View File

@ -42,9 +42,11 @@
#pragma once
#include <uORB/uORB.h>
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include <uORB/uORB.h>
#include "mavlink_stream.h"
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
@ -65,20 +67,75 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class Mavlink;
class MavlinkMissionManager {
class MavlinkMissionManager : public MavlinkStream {
public:
MavlinkMissionManager(Mavlink *parent);
~MavlinkMissionManager();
const char *get_name() const
{
return MavlinkMissionManager::get_name_static();
}
static const char *get_name_static()
{
return "MISSION_ITEM";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_MISSION_ITEM;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkMissionManager(mavlink);
}
unsigned get_size();
void handle_message(const mavlink_message_t *msg);
void set_verbose(bool v) { _verbose = v; }
private:
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
uint8_t _comp_id;
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
void init_offboard_mission();
int update_active_mission(int dataman_id, unsigned count, int seq);
void send_message(mavlink_message_t *msg);
/**
* @brief Sends an waypoint ack message
*/
@ -110,10 +167,6 @@ public:
*/
void send_mission_item_reached(uint16_t seq);
void eventloop();
void handle_message(const mavlink_message_t *msg);
void handle_mission_ack(const mavlink_message_t *msg);
void handle_mission_set_current(const mavlink_message_t *msg);
@ -138,43 +191,8 @@ public:
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void set_verbose(bool v) { _verbose = v; }
protected:
explicit MavlinkMissionManager(Mavlink *mavlink);
private:
Mavlink* _mavlink;
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
mavlink_channel_t _channel;
uint8_t _comp_id;
/* do not allow copying this class */
MavlinkMissionManager(const MavlinkMissionManager&);
MavlinkMissionManager operator=(const MavlinkMissionManager&);
void send(const hrt_abstime t);
};

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

@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
_last_sent = (t / _interval) * _interval;
_last_sent = t;
return true;
}