fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-27 13:54:55 +04:00
parent 18f72f8dd7
commit 141982a3ac
9 changed files with 371 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+40 -28
View File
@@ -79,6 +79,7 @@
#include "mavlink_main.h"
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
#include "mavlink_rate_limiter.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -321,7 +322,7 @@ Mavlink::get_uart_fd()
mavlink_channel_t
Mavlink::get_channel()
{
return _chan;
return _channel;
}
/****************************************************************************
@@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
}
@@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
}
}
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size)
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size)
{
/* check if already subscribed to this topic */
MavlinkOrbSubscription *sub;
@@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[])
int ch;
_baudrate = 57600;
_chan = MAVLINK_COMM_0;
_channel = MAVLINK_COMM_0;
_mode = MODE_OFFBOARD;
@@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[])
thread_running = true;
unsigned lowspeed_counter = 0;
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));
@@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[])
warnx("started");
mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* add default streams, intervals depend on baud rate */
/* add default streams depending on mode, intervals depend on baud rate */
float rate_mult = _baudrate / 57600.0f;
if (rate_mult > 4.0f) {
rate_mult = 4.0f;
}
add_stream("HEARTBEAT", 1.0f);
add_stream("SYS_STATUS", 1.0f * rate_mult);
add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
add_stream("HIGHRES_IMU", 1.0f * rate_mult);
add_stream("ATTITUDE", 10.0f * rate_mult);
add_stream("GPS_RAW_INT", 1.0f * rate_mult);
add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
switch(_mode) {
case MODE_OFFBOARD:
add_stream("SYS_STATUS", 1.0f * rate_mult);
add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
add_stream("HIGHRES_IMU", 1.0f * rate_mult);
add_stream("ATTITUDE", 10.0f * rate_mult);
add_stream("GPS_RAW_INT", 1.0f * rate_mult);
add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult);
break;
case MODE_HIL:
add_stream("SYS_STATUS", 1.0f * rate_mult);
add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
add_stream("HIGHRES_IMU", 1.0f * rate_mult);
add_stream("ATTITUDE", 10.0f * rate_mult);
add_stream("GPS_RAW_INT", 1.0f * rate_mult);
add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
break;
default:
break;
}
MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult);
while (!_task_should_exit) {
/* main loop */
@@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
/* 0.5 Hz */
if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
}
lowspeed_counter++;
bool updated;
orb_check(mission_result_sub, &updated);
@@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[])
}
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} else {
if (slow_rate_limiter.check(t)) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
}
}
if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) {
/* send parameters at 20 Hz (if queued for sending) */
if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(hrt_absolute_time());
if (_baudrate > 57600) {
mavlink_pm_queued_send();
}
/* send one string at 20 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
src/modules/mavlink/mavlink_main.h
+3 -6
View File
@@ -201,7 +201,7 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size);
mavlink_channel_t get_channel();
@@ -231,10 +231,7 @@ private:
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _chan;
// XXX probably should be in a header...
// extern pthread_t receive_start(int uart);
mavlink_channel_t _channel;
struct mavlink_logbuffer lb;
unsigned int total_counter;
@@ -248,7 +245,7 @@ private:
bool _verbose;
int _uart;
int _baudrate;
bool gcs_link;
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
src/modules/mavlink/mavlink_messages.cpp
+256 -182
View File
@@ -5,6 +5,7 @@
* Author: ton
*/
#include <stdio.h>
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>
@@ -19,6 +20,10 @@
#include "mavlink_messages.h"
static uint16_t cm_uint16_from_m_float(float m);
static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
uint16_t
cm_uint16_from_m_float(float m)
{
@@ -32,6 +37,83 @@ cm_uint16_from_m_float(float m)
return (uint16_t)(m * 100.0f);
}
void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
*mavlink_state = 0;
*mavlink_base_mode = 0;
*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;
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;
/* 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;
}
}
class MavlinkStreamHeartbeat : public MavlinkStream {
public:
const char *get_name()
@@ -68,78 +150,13 @@ protected:
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;
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;
}
}
/* 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;
}
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
custom_mode.data,
mavlink_custom_mode,
mavlink_state);
}
@@ -164,7 +181,6 @@ private:
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));
@@ -215,7 +231,6 @@ private:
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));
@@ -281,7 +296,6 @@ private:
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));
@@ -313,11 +327,9 @@ public:
private:
MavlinkOrbSubscription *gps_sub;
struct vehicle_gps_position_s *gps;
protected:
void subscribe(Mavlink *mavlink)
{
gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s));
@@ -356,13 +368,12 @@ public:
private:
MavlinkOrbSubscription *pos_sub;
MavlinkOrbSubscription *home_sub;
struct vehicle_global_position_s *pos;
MavlinkOrbSubscription *home_sub;
struct home_position_s *home;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s));
@@ -404,11 +415,9 @@ public:
private:
MavlinkOrbSubscription *pos_sub;
struct vehicle_local_position_s *pos;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s));
@@ -444,11 +453,9 @@ public:
private:
MavlinkOrbSubscription *home_sub;
struct home_position_s *home;
protected:
void subscribe(Mavlink *mavlink)
{
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
@@ -466,6 +473,171 @@ protected:
};
class MavlinkStreamServoOutputRaw : public MavlinkStream {
public:
MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
{
sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
}
const char *get_name()
{
return _name;
}
MavlinkStream *new_instance()
{
return new MavlinkStreamServoOutputRaw(_n);
}
private:
MavlinkOrbSubscription *act_sub;
struct actuator_outputs_s *act;
char _name[20];
unsigned int _n;
protected:
void subscribe(Mavlink *mavlink)
{
orb_id_t act_topics[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s));
act = (struct actuator_outputs_s *)act_sub->get_data();
}
void send(const hrt_abstime t) {
act_sub->update(t);
mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000,
_n,
act->output[0],
act->output[1],
act->output[2],
act->output[3],
act->output[4],
act->output[5],
act->output[6],
act->output[7]);
}
};
class MavlinkStreamHILControls : public MavlinkStream {
public:
const char *get_name()
{
return "HIL_CONTROLS";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamHILControls();
}
private:
MavlinkOrbSubscription *status_sub;
struct vehicle_status_s *status;
MavlinkOrbSubscription *pos_sp_triplet_sub;
struct position_setpoint_triplet_s *pos_sp_triplet;
MavlinkOrbSubscription *act_sub;
struct actuator_outputs_s *act;
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 *)pos_sp_triplet_sub->get_data();
act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s));
act = (struct actuator_outputs_s *)act_sub->get_data();
}
void send(const hrt_abstime t) {
status_sub->update(t);
pos_sp_triplet_sub->update(t);
act_sub->update(t);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* HIL message as per MAVLink spec */
/* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
-1,
-1,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
((act->output[6] - 900.0f) / 600.0f) / 2.0f,
((act->output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_base_mode,
0);
} else {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
(act->output[0] - 1500.0f) / 500.0f,
(act->output[1] - 1500.0f) / 500.0f,
(act->output[2] - 1500.0f) / 500.0f,
(act->output[3] - 1000.0f) / 1000.0f,
(act->output[4] - 1500.0f) / 500.0f,
(act->output[5] - 1500.0f) / 500.0f,
(act->output[6] - 1500.0f) / 500.0f,
(act->output[7] - 1500.0f) / 500.0f,
mavlink_base_mode,
0);
}
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@@ -475,6 +647,11 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamGlobalPositionInt(),
new MavlinkStreamLocalPositionNED(),
new MavlinkStreamGPSGlobalOrigin(),
new MavlinkStreamServoOutputRaw(0),
new MavlinkStreamServoOutputRaw(1),
new MavlinkStreamServoOutputRaw(2),
new MavlinkStreamServoOutputRaw(3),
new MavlinkStreamHILControls(),
nullptr
};
@@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = {
//void
//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
//{
@@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = {
//}
//
//void
//MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
//{
// struct actuator_outputs_s act_outputs;
//
// orb_id_t ids[] = {
// ORB_ID(actuator_outputs_0),
// ORB_ID(actuator_outputs_1),
// ORB_ID(actuator_outputs_2),
// ORB_ID(actuator_outputs_3)
// };
//
// /* copy actuator data into local buffer */
// orb_copy(ids[l->arg], *l->subp, &act_outputs);
//
// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000,
// l->arg /* port number - needs GCS support */,
// /* QGC has port number support already */
// act_outputs.output[0],
// act_outputs.output[1],
// act_outputs.output[2],
// act_outputs.output[3],
// act_outputs.output[4],
// act_outputs.output[5],
// act_outputs.output[6],
// act_outputs.output[7]);
//
// /* only send in HIL mode and only send first group for HIL */
// if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
//
// /* 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);
//
// /* HIL message as per MAVLink spec */
//
// /* scale / assign outputs depending on system type */
//
// if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
// mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
// hrt_absolute_time(),
// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
// -1,
// -1,
// -1,
// -1,
// mavlink_base_mode,
// 0);
//
// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
// mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
// hrt_absolute_time(),
// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
// -1,
// -1,
// mavlink_base_mode,
// 0);
//
// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
// mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
// hrt_absolute_time(),
// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
// ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
// mavlink_base_mode,
// 0);
//
// } else {
// mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
// hrt_absolute_time(),
// (act_outputs.output[0] - 1500.0f) / 500.0f,
// (act_outputs.output[1] - 1500.0f) / 500.0f,
// (act_outputs.output[2] - 1500.0f) / 500.0f,
// (act_outputs.output[3] - 1000.0f) / 1000.0f,
// (act_outputs.output[4] - 1500.0f) / 500.0f,
// (act_outputs.output[5] - 1500.0f) / 500.0f,
// (act_outputs.output[6] - 1500.0f) / 500.0f,
// (act_outputs.output[7] - 1500.0f) / 500.0f,
// mavlink_base_mode,
// 0);
// }
// }
// }
//}
//
//void
//MavlinkOrbListener::l_actuator_armed(const struct listener *l)
//{
// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
src/modules/mavlink/mavlink_orb_subscription.cpp
+1 -1
View File
@@ -13,7 +13,7 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
{
_data = malloc(size);
memset(_data, 0, size);
src/modules/mavlink/mavlink_orb_subscription.h
+2 -2
View File
@@ -16,7 +16,7 @@ class MavlinkOrbSubscription {
public:
MavlinkOrbSubscription *next;
MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size);
MavlinkOrbSubscription(const orb_id_t topic, size_t size);
~MavlinkOrbSubscription();
bool update(const hrt_abstime t);
@@ -24,7 +24,7 @@ public:
const struct orb_metadata *get_topic();
private:
const struct orb_metadata *_topic;
const orb_id_t _topic;
int _fd;
void *_data;
hrt_abstime _last_check;
src/modules/mavlink/mavlink_rate_limiter.cpp
+38
View File
@@ -0,0 +1,38 @@
/*
* mavlink_rate_limiter.cpp
*
* Created on: 26.02.2014
* Author: ton
*/
#include "mavlink_rate_limiter.h"
MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
{
}
MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000)
{
}
MavlinkRateLimiter::~MavlinkRateLimiter()
{
}
void
MavlinkRateLimiter::set_interval(unsigned int interval)
{
_interval = interval * 1000;
}
bool
MavlinkRateLimiter::check(hrt_abstime t)
{
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
_last_sent = (t / _interval) * _interval;
return true;
}
return false;
}
src/modules/mavlink/mavlink_rate_limiter.h
+28
View File
@@ -0,0 +1,28 @@
/*
* mavlink_rate_limiter.h
*
* Created on: 26.02.2014
* Author: ton
*/
#ifndef MAVLINK_RATE_LIMITER_H_
#define MAVLINK_RATE_LIMITER_H_
#include <drivers/drv_hrt.h>
class MavlinkRateLimiter {
private:
hrt_abstime _last_sent;
hrt_abstime _interval;
public:
MavlinkRateLimiter();
MavlinkRateLimiter(unsigned int interval);
~MavlinkRateLimiter();
void set_interval(unsigned int interval);
bool check(hrt_abstime t);
};
#endif /* MAVLINK_RATE_LIMITER_H_ */
src/modules/mavlink/mavlink_stream.cpp
+1 -1
View File
@@ -10,7 +10,7 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
{
}
src/modules/mavlink/module.mk
+2 -1
View File
@@ -41,6 +41,7 @@ SRCS += mavlink_main.cpp \
mavlink_receiver.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp
mavlink_stream.cpp \
mavlink_rate_limiter.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink