aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink_ftp.cpp10
-rw-r--r--src/modules/mavlink/mavlink_ftp.h50
-rw-r--r--src/modules/mavlink/mavlink_main.cpp98
-rw-r--r--src/modules/mavlink/mavlink_main.h2
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp37
-rw-r--r--src/modules/mavlink/mavlink_mission.cpp21
-rw-r--r--src/modules/mavlink/mavlink_mission.h2
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp92
-rw-r--r--src/modules/mavlink/mavlink_receiver.h2
9 files changed, 195 insertions, 119 deletions
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 00c8df18c..5b65dc369 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req)
uint32_t messageCRC;
// basic sanity checks; must validate length before use
- if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+ if (hdr->size > kMaxDataLength) {
errorCode = kErrNoRequest;
goto out;
}
@@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req)
// check request CRC to make sure this is one of ours
messageCRC = hdr->crc32;
hdr->crc32 = 0;
+ hdr->padding[0] = 0;
+ hdr->padding[1] = 0;
+ hdr->padding[2] = 0;
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
@@ -199,10 +202,13 @@ MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
- hdr->magic = kProtocolMagic;
+ hdr->seqNumber = req->header()->seqNumber + 1;
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
+ hdr->padding[0] = 0;
+ hdr->padding[1] = 0;
+ hdr->padding[2] = 0;
hdr->crc32 = crc32(req->rawData(), req->dataSize());
// then pack and send the reply back to the request source
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 43de89de9..1dd8f102e 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -38,9 +38,6 @@
*
* MAVLink remote file server.
*
- * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
- * a session ID and sequence number.
- *
* A limited number of requests (currently 2) may be outstanding at a time.
* Additional messages will be discarded.
*
@@ -74,16 +71,19 @@ private:
static MavlinkFTP *_server;
+ /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
+ /// structure ourselves to 32 bit alignment which should get us what we want.
struct RequestHeader
- {
- uint8_t magic;
- uint8_t session;
- uint8_t opcode;
- uint8_t size;
- uint32_t crc32;
- uint32_t offset;
+ {
+ uint16_t seqNumber; ///< sequence number for message
+ uint8_t session; ///< Session id for read and write commands
+ uint8_t opcode; ///< Command opcode
+ uint8_t size; ///< Size of data
+ uint8_t padding[3];
+ uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0
+ uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[];
- };
+ };
enum Opcode : uint8_t
{
@@ -131,10 +131,11 @@ private:
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
- if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+ if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+ _systemId = fromMessage->sysid;
_mavlink = mavlink;
- mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
- return true;
+ mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
+ return _message.target_system == _mavlink->get_system_id();
}
return false;
}
@@ -145,8 +146,14 @@ private:
// flat memory architecture, as we're operating between threads here.
mavlink_message_t msg;
msg.checksum = 0;
- unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
- _mavlink->get_channel(), &msg, sequence()+1, rawData());
+ unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id
+ _mavlink->get_component_id(), // Sender component id
+ _mavlink->get_channel(), // Channel to send on
+ &msg, // Message to pack payload into
+ 0, // Target network
+ _systemId, // Target system id
+ 0, // Target component id
+ rawData()); // Payload to pack into message
_mavlink->lockMessageBufferMutex();
bool success = _mavlink->message_buffer_write(&msg, len);
@@ -167,26 +174,25 @@ private:
#endif
}
- uint8_t *rawData() { return &_message.data[0]; }
- RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+ uint8_t *rawData() { return &_message.payload[0]; }
+ RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
- uint16_t sequence() const { return _message.seqnr; }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
- mavlink_encapsulated_data_t _message;
+ mavlink_file_transfer_protocol_t _message;
+ uint8_t _systemId;
};
- static const uint8_t kProtocolMagic = 'f';
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
- static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+ static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
/// Request worker; runs on the low-priority work queue to service
/// remote requests.
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 408605939..940e64144 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -134,44 +134,44 @@ Mavlink::Mavlink() :
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
- _total_counter(0),
- _receive_thread {},
- _verbose(false),
- _forwarding_on(false),
- _passing_on(false),
- _ftp_on(false),
- _uart_fd(-1),
- _baudrate(57600),
- _datarate(1000),
- _datarate_events(500),
- _rate_mult(1.0f),
- _mavlink_param_queue_index(0),
- mavlink_link_termination_allowed(false),
- _subscribe_to_stream(nullptr),
- _subscribe_to_stream_rate(0.0f),
- _flow_control_enabled(true),
- _last_write_success_time(0),
- _last_write_try_time(0),
- _bytes_tx(0),
- _bytes_txerr(0),
- _bytes_rx(0),
- _bytes_timestamp(0),
- _rate_tx(0.0f),
- _rate_txerr(0.0f),
- _rate_rx(0.0f),
- _rstatus {},
- _message_buffer {},
- _message_buffer_mutex {},
- _send_mutex {},
- _param_initialized(false),
- _param_system_id(0),
- _param_component_id(0),
- _param_system_type(0),
- _param_use_hil_gps(0),
-
- /* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
- _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+ _total_counter(0),
+ _receive_thread {},
+ _verbose(false),
+ _forwarding_on(false),
+ _passing_on(false),
+ _ftp_on(false),
+ _uart_fd(-1),
+ _baudrate(57600),
+ _datarate(1000),
+ _datarate_events(500),
+ _rate_mult(1.0f),
+ _mavlink_param_queue_index(0),
+ mavlink_link_termination_allowed(false),
+ _subscribe_to_stream(nullptr),
+ _subscribe_to_stream_rate(0.0f),
+ _flow_control_enabled(true),
+ _last_write_success_time(0),
+ _last_write_try_time(0),
+ _bytes_tx(0),
+ _bytes_txerr(0),
+ _bytes_rx(0),
+ _bytes_timestamp(0),
+ _rate_tx(0.0f),
+ _rate_txerr(0.0f),
+ _rate_rx(0.0f),
+ _rstatus {},
+ _message_buffer {},
+ _message_buffer_mutex {},
+ _send_mutex {},
+ _param_initialized(false),
+ _param_system_id(0),
+ _param_component_id(0),
+ _param_system_type(0),
+ _param_use_hil_gps(0),
+
+ /* performance counters */
+ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+ _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
+
+ _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
@@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
- _mode = MAVLINK_MODE_CAMERA;
+ // left in here for compatibility
+ _mode = MAVLINK_MODE_ONBOARD;
+ } else if (strcmp(optarg, "onboard") == 0) {
+ _mode = MAVLINK_MODE_ONBOARD;
}
break;
@@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
- case MAVLINK_MODE_CAMERA:
- warnx("mode: CAMERA");
+ case MAVLINK_MODE_ONBOARD:
+ warnx("mode: ONBOARD");
break;
default:
@@ -1391,13 +1396,14 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
+ configure_stream("OPTICAL_FLOW", 20.0f);
break;
- case MAVLINK_MODE_CAMERA:
+ case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
- configure_stream("ATTITUDE", 15.0f);
- configure_stream("GLOBAL_POSITION_INT", 15.0f);
- configure_stream("CAMERA_CAPTURE", 1.0f);
+ configure_stream("ATTITUDE", 50.0f);
+ configure_stream("GLOBAL_POSITION_INT", 50.0f);
+ configure_stream("CAMERA_CAPTURE", 2.0f);
break;
default:
@@ -1653,6 +1659,8 @@ Mavlink::display_status()
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
+ printf("\tmavlink chan: #%u\n", _channel);
+
if (_rstatus.timestamp > 0) {
printf("\ttype:\t\t");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1e2e94cef..7f9d225bb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -127,7 +127,7 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
- MAVLINK_MODE_CAMERA
+ MAVLINK_MODE_ONBOARD
};
void set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index af4d46a36..a2f3828ff 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -774,6 +774,9 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
+ MavlinkOrbSubscription *_sensor_combined_sub;
+ uint64_t _sensor_combined_time;
+
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -789,7 +792,9 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
- _airspeed_time(0)
+ _airspeed_time(0),
+ _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+ _sensor_combined_time(0)
{}
void send(const hrt_abstime t)
@@ -799,12 +804,14 @@ protected:
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
+ struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
+ updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) {
mavlink_vfr_hud_t msg;
@@ -813,7 +820,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
- msg.alt = pos.alt;
+ msg.alt = sensor_combined.baro_alt_meter;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -879,8 +886,8 @@ protected:
msg.eph = cm_uint16_from_m_float(gps.eph);
msg.epv = cm_uint16_from_m_float(gps.epv);
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
- msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- msg.satellites_visible = gps.satellites_used;
+ msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+ msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
@@ -950,11 +957,11 @@ protected:
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
msg.alt = pos.alt * 1000.0f;
- msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
- msg.vx = pos.vel_n * 100.0f;
- msg.vy = pos.vel_e * 100.0f;
- msg.vz = pos.vel_d * 100.0f;
- msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+ msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+ msg.vx = pos.vel_n * 100.0f;
+ msg.vy = pos.vel_e * 100.0f;
+ msg.vz = pos.vel_d * 100.0f;
+ msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
}
@@ -1015,9 +1022,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
- msg.vx = pos.vx;
- msg.vy = pos.vy;
- msg.vz = pos.vz;
+ msg.vx = pos.vx;
+ msg.vy = pos.vy;
+ msg.vz = pos.vz;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
}
@@ -1078,9 +1085,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
- msg.roll = pos.roll;
- msg.pitch = pos.pitch;
- msg.yaw = pos.yaw;
+ msg.roll = pos.roll;
+ msg.pitch = pos.pitch;
+ msg.yaw = pos.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
}
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 7a761588a..d436c95e9 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -58,6 +58,10 @@
#endif
static const int ERROR = -1;
+#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
+ ((_msg.target_component == mavlink_system.compid) || \
+ (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
+ (_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(_interval / 10.0f),
- _verbose(false),
- _comp_id(MAV_COMP_ID_MISSIONPLANNER)
+ _verbose(false)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
- if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpa)) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
- if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
- if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wprl)) {
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
- if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpr)) {
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
- if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
- if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+ if (CHECK_SYSID_COMPID_MISSION(wp)) {
if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
@@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
- if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+ if (CHECK_SYSID_COMPID_MISSION(wpca)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
/* don't touch mission items storage itself, but only items count in mission state */
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 8ff27f87d..a7bb94c22 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -126,8 +126,6 @@ private:
bool _verbose;
- uint8_t _comp_id;
-
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0fae0a2f..bed1bd789 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
- _radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_long(msg);
break;
+ case MAVLINK_MSG_ID_COMMAND_INT:
+ handle_message_command_int(msg);
+ break;
+
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
break;
@@ -169,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_data_stream(msg);
break;
- case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+ case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
@@ -277,6 +280,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
void
+MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
+{
+ /* command */
+ mavlink_command_int_t cmd_mavlink;
+ mavlink_msg_command_int_decode(msg, &cmd_mavlink);
+
+ if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+ || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+ //check for MAVLINK terminate command
+ if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+ /* This is the link shutdown command, terminate mavlink */
+ warnx("terminated by remote command");
+ fflush(stdout);
+ usleep(50000);
+
+ /* terminate other threads and this thread */
+ _mavlink->_task_should_exit = true;
+
+ } else {
+
+ if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+ warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+ mavlink_system.sysid, mavlink_system.compid);
+ return;
+ }
+
+ struct vehicle_command_s vcmd;
+ memset(&vcmd, 0, sizeof(vcmd));
+
+ /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
+ vcmd.param1 = cmd_mavlink.param1;
+ vcmd.param2 = cmd_mavlink.param2;
+ vcmd.param3 = cmd_mavlink.param3;
+ vcmd.param4 = cmd_mavlink.param4;
+ /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
+ vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
+ vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
+ vcmd.param7 = cmd_mavlink.z;
+ // XXX do proper translation
+ vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
+ vcmd.target_system = cmd_mavlink.target_system;
+ vcmd.target_component = cmd_mavlink.target_component;
+ vcmd.source_system = msg->sysid;
+ vcmd.source_component = msg->compid;
+
+ if (_cmd_pub < 0) {
+ _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+ } else {
+ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+ }
+ }
+ }
+}
+
+void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
{
/* optical flow */
@@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
-
- /* this means that heartbeats alone won't be published to the radio status no more */
- _radio_status_available = true;
}
}
@@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
- hrt_abstime tnow = hrt_absolute_time();
-
- /* always set heartbeat, publish only if telemetry link not up */
- tstatus.heartbeat_time = tnow;
-
- /* if no radio status messages arrive, lets at least publish that heartbeats were received */
- if (!_radio_status_available) {
+ /* set heartbeat time and topic time and publish -
+ * the telem status also gets updated on telemetry events
+ */
+ tstatus.timestamp = hrt_absolute_time();
+ tstatus.heartbeat_time = tstatus.timestamp;
- tstatus.timestamp = tnow;
- /* telem_time indicates the timestamp of a telemetry status packet and we got none */
- tstatus.telem_time = 0;
- tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+ if (_telemetry_status_pub < 0) {
+ _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
- if (_telemetry_status_pub < 0) {
- _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
-
- } else {
- orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
- }
+ } else {
+ orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 014193100..91125736c 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -110,6 +110,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
+ void handle_message_command_int(mavlink_message_t *msg);
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -151,7 +152,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
- bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-27 15:06:17 +0000