fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 23:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master' into offboard2_externalsetpointmessages

Browse Source 
Conflicts:
	src/modules/mavlink/mavlink_receiver.cpp
This commit is contained in:
Thomas Gubler
2014-08-13 15:07:09 +02:00
parent 85eed22150 54fc6aa678
commit e634c9fa99
19 changed files with 709 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_ftp.cpp
+30 -12
View File
@@ -39,6 +39,9 @@
#include "mavlink_ftp.h"
// Uncomment the line below to get better debug output. Never commit with this left on.
//#define MAVLINK_FTP_DEBUG
MavlinkFTP *MavlinkFTP::_server;
MavlinkFTP *
@@ -125,7 +128,9 @@ MavlinkFTP::_worker(Request *req)
warnx("ftp: bad crc");
}
//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
#endif
switch (hdr->opcode) {
case kCmdNone:
@@ -172,7 +177,9 @@ out:
// handle success vs. error
if (errorCode == kErrNone) {
hdr->opcode = kRspAck;
//warnx("FTP: ack\n");
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: ack\n");
#endif
} else {
warnx("FTP: nak %u", errorCode);
hdr->opcode = kRspNak;
@@ -217,7 +224,9 @@ MavlinkFTP::_workList(Request *req)
return kErrNotDir;
}
//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: list %s offset %d", dirPath, hdr->offset);
#endif
ErrorCode errorCode = kErrNone;
struct dirent entry, *result = nullptr;
@@ -247,11 +256,13 @@ MavlinkFTP::_workList(Request *req)
uint32_t fileSize = 0;
char buf[256];
char direntType;
// store the type marker
// Determine the directory entry type
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
// For files we get the file size as well
direntType = kDirentFile;
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
struct stat st;
if (stat(buf, &st) == 0) {
@@ -259,29 +270,34 @@ MavlinkFTP::_workList(Request *req)
}
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
direntType = kDirentDir;
break;
default:
hdr->data[offset++] = kDirentUnknown;
direntType = kDirentUnknown;
break;
}
if (entry.d_type == DTYPE_FILE) {
// Files send filename and file length
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
} else {
// Everything else just sends name
strncpy(buf, entry.d_name, sizeof(buf));
buf[sizeof(buf)-1] = 0;
}
size_t nameLen = strlen(buf);
// name too big to fit?
if ((nameLen + offset + 2) > kMaxDataLength) {
// Do we have room for the name, the one char directory identifier and the null terminator?
if ((offset + nameLen + 2) > kMaxDataLength) {
break;
}
// copy the name, which we know will fit
// Move the data into the buffer
hdr->data[offset++] = direntType;
strcpy((char *)&hdr->data[offset], buf);
//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
#ifdef MAVLINK_FTP_DEBUG
printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
#endif
offset += nameLen + 1;
}
@@ -342,7 +358,9 @@ MavlinkFTP::_workRead(Request *req)
}
// Seek to the specified position
//warnx("seek %d", hdr->offset);
#ifdef MAVLINK_FTP_DEBUG
warnx("seek %d", hdr->offset);
#endif
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
warnx("seek fail");
src/modules/mavlink/mavlink_ftp.h
+11 -9
View File
@@ -154,15 +154,17 @@ private:
if (!success) {
warnx("FTP TX ERR");
}
// else {
// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
// _mavlink->get_system_id(),
// _mavlink->get_component_id(),
// _mavlink->get_channel(),
// len,
// msg.checksum);
// }
}
#ifdef MAVLINK_FTP_DEBUG
else {
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
_mavlink->get_system_id(),
_mavlink->get_component_id(),
_mavlink->get_channel(),
len,
msg.checksum);
}
#endif
}
uint8_t *rawData() { return &_message.data[0]; }
src/modules/mavlink/mavlink_receiver.cpp
+46
View File
@@ -110,6 +110,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_force_sp_pub(-1),
_pos_sp_triplet_pub(-1),
_vicon_position_pub(-1),
_vision_position_pub(-1),
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
@@ -157,6 +158,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
handle_message_set_attitude_target(msg);
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate(msg);
break;
case MAVLINK_MSG_ID_RADIO_STATUS:
handle_message_radio_status(msg);
break;
@@ -523,6 +529,45 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
}
}
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(msg, &pos);
struct vision_position_estimate vision_position;
memset(&vision_position, 0, sizeof(vision_position));
// Use the component ID to identify the vision sensor
vision_position.id = msg->compid;
vision_position.timestamp_boot = hrt_absolute_time();
vision_position.timestamp_computer = pos.usec;
vision_position.x = pos.x;
vision_position.y = pos.y;
vision_position.z = pos.z;
// XXX fix this
vision_position.vx = 0.0f;
vision_position.vy = 0.0f;
vision_position.vz = 0.0f;
math::Quaternion q;
q.from_euler(pos.roll, pos.pitch, pos.yaw);
vision_position.q[0] = q(0);
vision_position.q[1] = q(1);
vision_position.q[2] = q(2);
vision_position.q[3] = q(3);
if (_vision_position_pub < 0) {
_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
} else {
orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position);
}
}
void
MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
{
@@ -610,6 +655,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}
}
}
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -58,6 +58,7 @@
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -113,6 +114,7 @@ private:
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);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
void handle_message_set_attitude_target(mavlink_message_t *msg);
@@ -149,6 +151,7 @@ private:
orb_advert_t _force_sp_pub;
orb_advert_t _pos_sp_triplet_pub;
orb_advert_t _vicon_position_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;