zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

sbg
Anton Babushkin 11 years ago
parent
18f72f8dd7
commit
141982a3ac
9 changed files with 371 additions and 221 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 68
      src/modules/mavlink/mavlink_main.cpp
  2. 9
      src/modules/mavlink/mavlink_main.h
  3. 438
      src/modules/mavlink/mavlink_messages.cpp
  4. 2
      src/modules/mavlink/mavlink_orb_subscription.cpp
  5. 4
      src/modules/mavlink/mavlink_orb_subscription.h
  6. 38
      src/modules/mavlink/mavlink_rate_limiter.cpp
  7. 28
      src/modules/mavlink/mavlink_rate_limiter.h
  8. 2
      src/modules/mavlink/mavlink_stream.cpp
  9. 3
      src/modules/mavlink/module.mk

68
src/modules/mavlink/mavlink_main.cpp
Unescape View File

@ -79,6 +79,7 @@ @@ -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() @@ -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) @@ -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) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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);

9
src/modules/mavlink/mavlink_main.h
Unescape View File

@ -201,7 +201,7 @@ public: @@ -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: @@ -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: @@ -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

438
src/modules/mavlink/mavlink_messages.cpp
Unescape View File

@ -5,6 +5,7 @@ @@ -5,6 +5,7 @@
* Author: ton
*/
#include <stdio.h>
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>
@ -19,6 +20,10 @@ @@ -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) @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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[] = { @@ -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[] = { @@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = {
//void
//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
//{
@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = { @@ -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);

2
src/modules/mavlink/mavlink_orb_subscription.cpp
Unescape View File

@ -13,7 +13,7 @@ @@ -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);

4
src/modules/mavlink/mavlink_orb_subscription.h
Unescape View File

@ -16,7 +16,7 @@ class MavlinkOrbSubscription { @@ -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: @@ -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;

38
src/modules/mavlink/mavlink_rate_limiter.cpp
Unescape View File

@ -0,0 +1,38 @@ @@ -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;
}

28
src/modules/mavlink/mavlink_rate_limiter.h
Unescape View File

@ -0,0 +1,28 @@ @@ -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_ */

2
src/modules/mavlink/mavlink_stream.cpp
Unescape View File

@ -10,7 +10,7 @@ @@ -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)
{
}

3
src/modules/mavlink/module.mk
Unescape View File

@ -41,6 +41,7 @@ SRCS += mavlink_main.cpp \ @@ -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

Write Preview
Loading…
Cancel
Save