diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index d7697cd673..cd9c05b136 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -287,8 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index bd956f7bf5..e31f40cd2a 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -323,8 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 512c6a0f23..7c76be7ec0 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -133,6 +133,8 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=n +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=1 CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n CONFIG_STM32_DAC=n diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 00c8df18c5..5b65dc369d 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 43de89de91..1dd8f102e7 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(&_message.data[0]); } + uint8_t *rawData() { return &_message.payload[0]; } + RequestHeader *header() { return reinterpret_cast(&_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_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a602344fd2..bed1bd7898 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -172,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; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 95c6ba13e0..a8485ee44e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -175,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) {