diff options
20 files changed, 906 insertions, 141 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index d6e638c0a..1f8d8b862 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 10000 -d /dev/ttyACM0 +mavlink start -r 10000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 usleep 100000 diff --git a/mavlink/include/mavlink/v1.0/checksum.h b/mavlink/include/mavlink/v1.0/checksum.h index 948e080a1..7d9b6ac0c 100644 --- a/mavlink/include/mavlink/v1.0/checksum.h +++ b/mavlink/include/mavlink/v1.0/checksum.h @@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) * @param data new bytes to hash * @param crcAccum the already accumulated checksum **/ -static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) +static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) { const uint8_t *p = (const uint8_t *)pBuffer; while (length--) { diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 020211d40..de6e22011 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -5,6 +5,10 @@ #ifndef COMMON_H #define COMMON_H +#ifndef MAVLINK_H + #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually. +#endif + #ifdef __cplusplus extern "C" { #endif diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 96672f847..1639a830b 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui #endif { // This code part is the same for all messages; - uint16_t checksum; msg->magic = MAVLINK_STX; msg->len = length; msg->sysid = system_id; @@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); + msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(crc_extra, &msg->checksum); #endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint buf[4] = mavlink_system.compid; buf[5] = msgid; status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); crc_accumulate_buffer(&checksum, packet, length); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); @@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); @@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m */ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h index 4019c619e..43658e629 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_types.h +++ b/mavlink/include/mavlink/v1.0/mavlink_types.h @@ -28,6 +28,7 @@ #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#pragma pack(push, 1) typedef struct param_union { union { float param_float; @@ -62,13 +63,12 @@ typedef struct __mavlink_message { uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; - typedef struct __mavlink_extended_message { mavlink_message_t base_msg; int32_t extended_payload_len; ///< Length of extended payload if any uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; } mavlink_extended_message_t; - +#pragma pack(pop) typedef enum { MAVLINK_TYPE_CHAR = 0, diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a0a18bc2e..46db788a6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index d2a231694..9894a34d7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; //xxx: param - /* input conditioning */ // warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 79184e2cd..fe03b8065 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, _rate_setpoint = 0.0f; if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison + if(fabsf(denumerator) > FLT_EPSILON) { _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; // warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } @@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3ff3d9922..49a892609 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* offset estimation - UNUSED */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); - /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r2 = param_find("EKF_ATT_V4_R2"); h->r3 = param_find("EKF_ATT_V4_R3"); - h->roll_off = param_find("ATT_ROLL_OFF3"); - h->pitch_off = param_find("ATT_PITCH_OFF3"); - h->yaw_off = param_find("ATT_YAW_OFF3"); - h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); @@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI / 180.0f; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 74a141609..5985541ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params { struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; - param_t roll_off, pitch_off, yaw_off; param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp new file mode 100644 index 000000000..f1436efb8 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include <crc32.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> + +#include "mavlink_ftp.h" + +MavlinkFTP *MavlinkFTP::_server; + +MavlinkFTP * +MavlinkFTP::getServer() +{ + // XXX this really cries out for some locking... + if (_server == nullptr) { + _server = new MavlinkFTP; + } + return _server; +} + +MavlinkFTP::MavlinkFTP() +{ + // initialise the request freelist + dq_init(&_workFree); + sem_init(&_lock, 0, 1); + + // initialize session list + for (size_t i=0; i<kMaxSession; i++) { + _session_fds[i] = -1; + } + + // drop work entries onto the free list + for (unsigned i = 0; i < kRequestQueueSize; i++) { + _qFree(&_workBufs[i]); + } + +} + +void +MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg) +{ + // get a free request + auto req = _dqFree(); + + // if we couldn't get a request slot, just drop it + if (req != nullptr) { + + // decode the request + if (req->decode(mavlink, msg)) { + + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } else { + _qFree(req); + } + } +} + +void +MavlinkFTP::_workerTrampoline(void *arg) +{ + auto req = reinterpret_cast<Request *>(arg); + auto server = MavlinkFTP::getServer(); + + // call the server worker with the work item + server->_worker(req); +} + +void +MavlinkFTP::_worker(Request *req) +{ + auto hdr = req->header(); + ErrorCode errorCode = kErrNone; + uint32_t messageCRC; + + // basic sanity checks; must validate length before use + if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + errorCode = kErrNoRequest; + goto out; + } + + // check request CRC to make sure this is one of ours + messageCRC = hdr->crc32; + hdr->crc32 = 0; + if (crc32(req->rawData(), req->dataSize()) != messageCRC) { + errorCode = kErrNoRequest; + goto out; + printf("ftp: bad crc\n"); + } + + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + + switch (hdr->opcode) { + case kCmdNone: + break; + + case kCmdTerminate: + errorCode = _workTerminate(req); + break; + + case kCmdReset: + errorCode = _workReset(); + break; + + case kCmdList: + errorCode = _workList(req); + break; + + case kCmdOpen: + errorCode = _workOpen(req, false); + break; + + case kCmdCreate: + errorCode = _workOpen(req, true); + break; + + case kCmdRead: + errorCode = _workRead(req); + break; + + case kCmdWrite: + errorCode = _workWrite(req); + break; + + case kCmdRemove: + errorCode = _workRemove(req); + break; + + default: + errorCode = kErrNoRequest; + break; + } + +out: + // handle success vs. error + if (errorCode == kErrNone) { + hdr->opcode = kRspAck; + printf("FTP: ack\n"); + } else { + printf("FTP: nak %u\n", errorCode); + hdr->opcode = kRspNak; + hdr->size = 1; + hdr->data[0] = errorCode; + } + + // respond to the request + _reply(req); + + // free the request buffer back to the freelist + _qFree(req); +} + +void +MavlinkFTP::_reply(Request *req) +{ + auto hdr = req->header(); + + // message is assumed to be already constructed in the request buffer, so generate the CRC + hdr->crc32 = 0; + hdr->crc32 = crc32(req->rawData(), req->dataSize()); + + // then pack and send the reply back to the request source + req->reply(); +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(Request *req) +{ + auto hdr = req->header(); + DIR *dp = opendir(req->dataAsCString()); + + if (dp == nullptr) { + printf("FTP: can't open path '%s'\n", req->dataAsCString()); + return kErrNotDir; + } + + ErrorCode errorCode = kErrNone; + struct dirent entry, *result = nullptr; + unsigned offset = 0; + + // move to the requested offset + seekdir(dp, hdr->offset); + + for (;;) { + // read the directory entry + if (readdir_r(dp, &entry, &result)) { + errorCode = kErrIO; + break; + } + + // no more entries? + if (result == nullptr) { + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + break; + } + + // name too big to fit? + if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { + break; + } + + // store the type marker + switch (entry.d_type) { + case DTYPE_FILE: + hdr->data[offset++] = kDirentFile; + break; + case DTYPE_DIRECTORY: + hdr->data[offset++] = kDirentDir; + break; + default: + hdr->data[offset++] = kDirentUnknown; + break; + } + + // copy the name, which we know will fit + strcpy((char *)&hdr->data[offset], entry.d_name); + offset += strlen(entry.d_name) + 1; + printf("FTP: list %s\n", entry.d_name); + } + + closedir(dp); + hdr->size = offset; + + return errorCode; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(Request *req, bool create) +{ + auto hdr = req->header(); + + int session_index = _findUnusedSession(); + if (session_index < 0) { + return kErrNoSession; + } + + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + int fd = ::open(req->dataAsCString(), oflag); + if (fd < 0) { + return create ? kErrPerm : kErrNotFile; + } + _session_fds[session_index] = fd; + + hdr->session = session_index; + hdr->size = 0; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(Request *req) +{ + auto hdr = req->header(); + + int session_index = hdr->session; + + if (!_validSession(session_index)) { + return kErrNoSession; + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); + if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + return kErrEOF; + } + + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof + return kErrIO; + } + + printf("Read success %d\n", bytes_read); + hdr->size = bytes_read; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(Request *req) +{ +#if 0 + // NYI: Coming soon + auto hdr = req->header(); + + // look up session + auto session = getSession(hdr->session); + if (session == nullptr) { + return kErrNoSession; + } + + // append to file + int result = session->append(hdr->offset, &hdr->data[0], hdr->size); + + if (result < 0) { + // XXX might also be no space, I/O, etc. + return kErrNotAppend; + } + + hdr->size = result; + return kErrNone; +#else + return kErrPerm; +#endif +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemove(Request *req) +{ + auto hdr = req->header(); + + // for now, send error reply + return kErrPerm; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(Request *req) +{ + auto hdr = req->header(); + + if (!_validSession(hdr->session)) { + return kErrNoSession; + } + + ::close(_session_fds[hdr->session]); + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(void) +{ + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] != -1) { + ::close(_session_fds[i]); + _session_fds[i] = -1; + } + } + + return kErrNone; +} + +bool +MavlinkFTP::_validSession(unsigned index) +{ + if ((index >= kMaxSession) || (_session_fds[index] < 0)) { + return false; + } + return true; +} + +int +MavlinkFTP::_findUnusedSession(void) +{ + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] == -1) { + return i; + } + } + + return -1; +} + +char * +MavlinkFTP::Request::dataAsCString() +{ + // guarantee nul termination + if (header()->size < kMaxDataLength) { + requestData()[header()->size] = '\0'; + } else { + requestData()[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)&(header()->data[0]); +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h new file mode 100644 index 000000000..8a1b3fbaa --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.h @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/** + * @file mavlink_ftp.h + * + * 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. + * + * Messages consist of a fixed header, followed by a data area. + * + */ + +#include <dirent.h> +#include <queue.h> + +#include <nuttx/wqueue.h> +#include <systemlib/err.h> + +#include "mavlink_messages.h" + +class MavlinkFTP +{ +public: + MavlinkFTP(); + + static MavlinkFTP *getServer(); + + // static interface + void handle_message(Mavlink* mavlink, + mavlink_message_t *msg); + +private: + + static const unsigned kRequestQueueSize = 2; + + static MavlinkFTP *_server; + + struct RequestHeader + { + uint8_t magic; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint32_t crc32; + uint32_t offset; + uint8_t data[]; + }; + + enum Opcode : uint8_t + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in <path> from <offset> + kCmdOpen, // opens <path> for reading, returns <session> + kCmdRead, // reads <size> bytes from <offset> in <session> + kCmdCreate, // creates <path> for writing, returns <session> + kCmdWrite, // appends <size> bytes at <offset> in <session> + kCmdRemove, // remove file (only if created by server?) + + kRspAck, + kRspNak + }; + + enum ErrorCode : uint8_t + { + kErrNone, + kErrNoRequest, + kErrNoSession, + kErrSequence, + kErrNotDir, + kErrNotFile, + kErrEOF, + kErrNotAppend, + kErrTooBig, + kErrIO, + kErrPerm + }; + + int _findUnusedSession(void); + bool _validSession(unsigned index); + + static const unsigned kMaxSession = 2; + int _session_fds[kMaxSession]; + + class Request + { + public: + union { + dq_entry_t entry; + work_s work; + }; + + bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { + if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + _mavlink = mavlink; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + return true; + } + return false; + } + + void reply() { + + // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the + // 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(), rawData()); + + _mavlink->lockMessageBufferMutex(); + bool fError = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!fError) { + 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); + } + } + + uint8_t *rawData() { return &_message.data[0]; } + RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[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; + + }; + + 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); + + /// Request worker; runs on the low-priority work queue to service + /// remote requests. + /// + static void _workerTrampoline(void *arg); + void _worker(Request *req); + + /// Reply to a request (XXX should be a Request method) + /// + void _reply(Request *req); + + ErrorCode _workList(Request *req); + ErrorCode _workOpen(Request *req, bool create); + ErrorCode _workRead(Request *req); + ErrorCode _workWrite(Request *req); + ErrorCode _workRemove(Request *req); + ErrorCode _workTerminate(Request *req); + ErrorCode _workReset(); + + // work freelist + Request _workBufs[kRequestQueueSize]; + dq_queue_t _workFree; + sem_t _lock; + + void _qLock() { do {} while (sem_wait(&_lock) != 0); } + void _qUnlock() { sem_post(&_lock); } + + void _qFree(Request *req) { + _qLock(); + dq_addlast(&req->entry, &_workFree); + _qUnlock(); + } + + Request *_dqFree() { + _qLock(); + auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree)); + _qUnlock(); + return req; + } + +}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ee5e7aa3a..9a5e31ef4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -83,6 +83,10 @@ #include "mavlink_rate_limiter.h" #include "mavlink_commands.h" +#ifndef MAVLINK_CRC_EXTRA + #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + Mavlink *instance; switch (channel) { @@ -231,6 +236,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _passing_on(false), + _ftp_on(false), _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), @@ -468,7 +474,7 @@ Mavlink::get_instance_id() return _instance_id; } -mavlink_channel_t +const mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -546,6 +552,16 @@ void Mavlink::mavlink_update_system(void) _use_hil_gps = (bool)use_hil_gps; } +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -1640,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::message_buffer_init(int size) { + _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; _message_buffer.data = (char*)malloc(_message_buffer.size); - return (_message_buffer.data == 0) ? ERROR : OK; + + int ret; + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + } else { + ret = OK; + } + + return ret; } void @@ -1772,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1828,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[]) _wait_to_transmit = true; break; + case 'x': + _ftp_on = true; + break; + default: err_flag = true; break; @@ -1893,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on) { - /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(300)) { + if (_passing_on || _ftp_on) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { errx(1, "can't allocate message buffer, exiting"); } @@ -2058,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* pass messages from other UARTs */ - if (_passing_on) { + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { bool is_part; - void *read_ptr; + uint8_t *read_ptr; + uint8_t *write_ptr; - /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr(&read_ptr, &is_part); + int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - /* write first part of buffer */ - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); - message_buffer_mark_read(available); + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t*)&msg; + + // Pull a single message from the buffer + int read_count = available; + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ - if (is_part) { - /* guard get ptr by mutex */ - pthread_mutex_lock(&_message_buffer_mutex); - available = message_buffer_get_ptr(&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } + + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -2133,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); - if (_passing_on) { + if (_passing_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -2284,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f74b7daa7..6777d56c3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,27 +123,41 @@ public: /** * Display the mavlink status. */ - void status(); + void status(); - static int stream(int argc, char *argv[]); + static int stream(int argc, char *argv[]); - static int instance_count(); + static int instance_count(); - static Mavlink *new_instance(); + static Mavlink *new_instance(); - static Mavlink *get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); - static int destroy_all_instances(); + static int destroy_all_instances(); - static bool instance_exists(const char *device_name, Mavlink *self); + static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); - static int get_uart_fd(unsigned index); + static int get_uart_fd(unsigned index); - int get_uart_fd(); + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); const char *_device_name; @@ -153,30 +167,30 @@ public: MAVLINK_MODE_CAMERA }; - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; } + bool get_hil_enabled() { return _hil_enabled; } - bool get_use_hil_gps() { return _use_hil_gps; } + bool get_use_hil_gps() { return _use_hil_gps; } - bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_flow_control_enabled() { return _flow_control_enabled; } - bool get_forwarding_on() { return _forwarding_on; } + bool get_forwarding_on() { return _forwarding_on; } /** * Handle waypoint related messages. */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); + static int start_helper(int argc, char *argv[]); /** * Handle parameter related messages. */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_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); + void get_mavlink_mode_and_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); /** * Enable / disable Hardware in the Loop simulation mode. @@ -186,36 +200,41 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_enabled(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); - int get_instance_id(); + int get_instance_id(); /** * Enable / disable hardware flow control. * * @param enabled True if hardware flow control should be enabled */ - int enable_flow_control(bool enabled); + int enable_flow_control(bool enabled); - mavlink_channel_t get_channel(); + const mavlink_channel_t get_channel(); void configure_stream_threadsafe(const char *stream_name, const float rate); - bool _task_should_exit; /**< if true, mavlink task should exit */ + bool _task_should_exit; /**< if true, mavlink task should exit */ - int get_mavlink_fd() { return _mavlink_fd; } + int get_mavlink_fd() { return _mavlink_fd; } MavlinkStream * get_streams() { return _streams; } const /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } /** * Count a transmision error @@ -223,62 +242,63 @@ public: void count_txerr(); protected: - Mavlink *next; + Mavlink *next; private: - int _instance_id; + int _instance_id; - int _mavlink_fd; - bool _task_running; + int _mavlink_fd; + bool _task_running; /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; + orb_advert_t _mission_pub; + struct mission_s mission; + MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; + uint8_t _mavlink_wpm_comp_id; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; + unsigned int _total_counter; - pthread_t _receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int _mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; - bool mavlink_link_termination_allowed; + bool mavlink_link_termination_allowed; - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; - bool _flow_control_enabled; + bool _flow_control_enabled; struct mavlink_message_buffer { int write_ptr; @@ -286,12 +306,13 @@ private: int size; char *data; }; + mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ bool _param_initialized; param_t _param_system_id; @@ -305,7 +326,7 @@ private: * @param param The parameter id to send. * @return zero on success, nonzero on failure. */ - int mavlink_pm_send_param(param_t param); + int mavlink_pm_send_param(param_t param); /** * Send one parameter identified by index. @@ -313,7 +334,7 @@ private: * @param index The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_pm_send_param_for_index(uint16_t index); /** * Send one parameter identified by name. @@ -321,14 +342,14 @@ private: * @param name The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_name(const char *name); + int mavlink_pm_send_param_for_name(const char *name); /** * Send a queue of parameters, one parameter per function call. * * @return zero on success, nonzero on failure */ - int mavlink_pm_queued_send(void); + int mavlink_pm_queued_send(void); /** * Start sending the parameter queue. @@ -338,12 +359,12 @@ private: * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(); + void mavlink_pm_start_queued_send(); - void mavlink_update_system(); + void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); @@ -369,8 +390,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2458dd23a..e1ebc16cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -259,8 +259,15 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - (void)status_sub->update(&status); - (void)pos_sp_triplet_sub->update(&pos_sp_triplet); + if (!status_sub->update(&status)) { + /* if topic update failed fill it with defaults */ + memset(&status, 0, sizeof(status)); + } + + if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + /* if topic update failed fill it with defaults */ + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + } uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 425fc79a5..bb977d277 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -114,6 +114,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_alt0(0.0) { memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + + // make sure the FTP server is started + (void)MavlinkFTP::getServer(); } MavlinkReceiver::~MavlinkReceiver() @@ -160,6 +163,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + MavlinkFTP::getServer()->handle_message(_mavlink, msg); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index af81e8a31..040a07480 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,8 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include "mavlink_ftp.h" + class Mavlink; class MavlinkReceiver diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fe..a4d8bfbfb 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -43,7 +43,8 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp + mavlink_commands.cpp \ + mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 999cf8bb3..c8a3ec8f0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,6 +243,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * Board rotation Y (Pitch) offset + * + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); + +/** + * Board rotation X (Roll) offset + * + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); + +/** + * Board rotation Z (YAW) offset + * + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); + +/** * External magnetometer rotation * * This parameter defines the rotation of the external magnetometer relative diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..16fcb4c26 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,7 +229,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +252,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -341,6 +343,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -587,6 +591,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -791,6 +800,18 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } |