From 662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 22:36:28 +0400 Subject: mavlink: REQUEST_DATA_STREAM hadling implemented --- src/modules/mavlink/mavlink_receiver.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/mavlink/mavlink_receiver.h') diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8ccb2a035..eab8f071d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); -- cgit v1.2.3 From 12390d7281985b7e3b6649fc9889e2e60a48dad1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 4 May 2014 11:19:26 -0700 Subject: WIP: Mavlink file server --- src/modules/mavlink/mavlink_ftp.cpp | 377 +++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 211 +++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 7 + src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/mavlink/module.mk | 3 +- 5 files changed, 599 insertions(+), 1 deletion(-) create mode 100644 src/modules/mavlink/mavlink_ftp.cpp create mode 100644 src/modules/mavlink/mavlink_ftp.h (limited to 'src/modules/mavlink/mavlink_receiver.h') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp new file mode 100644 index 000000000..4cb31640e --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -0,0 +1,377 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#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); + + // drop work entries onto the free list + for (unsigned i = 0; i < kRequestQueueSize; i++) { + _qFree(&_workBufs[i]); + } + +} + +void +MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) +{ + // get a free request + auto req = _dqFree(); + + // if we couldn't get a request slot, just drop it + if (req != nullptr) { + + // decode the request + req->decode(channel, msg); + + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } +} + +void +MavlinkFTP::_workerTrampoline(void *arg) +{ + auto req = reinterpret_cast(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->data(), req->dataSize()) != messageCRC) { + errorCode = kErrNoRequest; + goto out; + } + + switch (hdr->opcode) { + case kCmdNone: + break; + + case kCmdTerminate: + if (!Session::terminate(hdr->session)) { + errorCode = kErrNoRequest; + } + break; + + case kCmdReset: + Session::reset(); + 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; + } else { + 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->data(), req->dataSize()); + + // then pack and send the reply back to the request source + mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data()); +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(Request *req) +{ + auto hdr = req->header(); + + // open directory + + // seek in directory + + // read entries until buffer is full + + // send reply + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(Request *req, bool create) +{ + auto hdr = req->header(); + + // allocate session ID + int session = Session::allocate(); + if (session < 0) { + return kErrNoSession; + } + + // get the session to open the file + if (!Session::get(session)->open(req->dataAsCString(), create)) { + return create ? kErrPerm : kErrNotFile; + } + + // save the session ID in the reply + hdr->session = session; + hdr->size = 0; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(Request *req) +{ + auto hdr = req->header(); + + // look up session + auto session = Session::get(hdr->session); + if (session == nullptr) { + return kErrNoSession; + } + + // read from file + int result = session->read(hdr->offset, &hdr->data[0], hdr->size); + + if (result < 0) { + return kErrIO; + } + hdr->size = result; + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(Request *req) +{ + auto hdr = req->header(); + + // look up session + auto session = Session::get(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; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemove(Request *req) +{ + auto hdr = req->header(); + + // for now, send error reply + return kErrPerm; +} + +MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession]; + +int +MavlinkFTP::Session::allocate() +{ + for (unsigned i = 0; i < kMaxSession; i++) { + if (_sessions[i]._fd < 0) { + return i; + } + } + return -1; +} + +MavlinkFTP::Session * +MavlinkFTP::Session::get(unsigned index) +{ + if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) { + return nullptr; + } + return &_sessions[index]; +} + +void +MavlinkFTP::Session::terminate() +{ + // clean up aborted transfers? + if (_fd >= 0) { + close(_fd); + _fd = -1; + } +} + +bool +MavlinkFTP::Session::terminate(unsigned index) + { + Session *session = get(index); + + if (session == nullptr) { + return false; + } + + session->terminate(); + return true; +} + +void +MavlinkFTP::Session::reset() +{ + for (unsigned i = 0; i < kMaxSession; i++) { + terminate(i); + } +} + +bool +MavlinkFTP::Session::open(const char *path, bool create) +{ + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + _fd = open(path, oflag); + if (_fd < 0) { + return false; + } + return true; +} + +int +MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count) +{ + // can we seek to the location? + if (lseek(_fd, offset, SEEK_SET) < 0) { + return -1; + } + + return read(_fd, buf, count); +} + +int +MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count) +{ + // make sure that the requested offset matches our current position + off_t pos = lseek(_fd, 0, SEEK_CUR); + if (pos != offset) { + return -1; + } + return write(_fd, buf, count); +} + +char * +MavlinkFTP::Request::dataAsCString() +{ + // guarantee nul termination + if (header()->size < kMaxDataLength) { + data()[header()->size] = '\0'; + } else { + data()[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)data(); +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h new file mode 100644 index 000000000..a4f67793e --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.h @@ -0,0 +1,211 @@ +/**************************************************************************** + * + * 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 +#include + +#include + +#include "mavlink_messages.h" + +class MavlinkFTP +{ +public: + MavlinkFTP(); + + static MavlinkFTP *getServer(); + + // static interface + void handle_message(mavlink_message_t *msg, + mavlink_channel_t channel); + +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[]; + }; + + struct FileList + { + uint32_t fileSize; + uint8_t nameLength; + uint8_t name[]; + }; + + enum Opcode : uint8_t + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in from + kCmdOpen, // opens for reading, returns + kCmdRead, // reads bytes from in + kCmdCreate, // creates for writing, returns + kCmdWrite, // appends bytes at in + 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 + }; + + class Session + { + public: + Session() : _fd(-1) {} + + static int allocate(); + static Session *get(unsigned index); + static bool terminate(unsigned index); + static void reset(); + + void terminate(); + bool open(const char *path, bool create); + int read(off_t offset, uint8_t *buf, uint8_t count); + int append(off_t offset, uint8_t *buf, uint8_t count); + + private: + static const unsigned kMaxSession = 2; + static Session _sessions[kMaxSession]; + + int _fd; + }; + + class Request + { + public: + union { + dq_entry_t entry; + work_s work; + }; + mavlink_channel_t channel; + + void decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) { + channel = fromChannel; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + } + + RequestHeader *header() { return reinterpret_cast(&_message.data[0]); } + uint8_t *data() { return &_message.data[0]; } + unsigned dataSize() { return header()->size + sizeof(RequestHeader); } + uint16_t sequence() const { return _message.seqnr; } + + char *dataAsCString(); + + private: + mavlink_encapsulated_data_t _message; + + }; + + static const uint8_t kProtocolMagic = 'f'; + 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); + + // 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(dq_remfirst(&_workFree)); + _qUnlock(); + return req; + } +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c93c1c00..fd1abe5ee 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,6 +112,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() @@ -150,6 +153,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel()); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..36e6143ac 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,8 @@ #include #include +#include "mavlink_ftp.h" + class Mavlink; class MavlinkReceiver diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index dcca11977..c348a33db 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 -- cgit v1.2.3 From e0ed0625f81841194b4bd9b26c7e047a1f68a1fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Jun 2014 17:34:21 +0200 Subject: commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions --- src/modules/commander/commander.cpp | 90 +++++++++----------- src/modules/commander/state_machine_helper.cpp | 112 +++++++++++++++---------- src/modules/commander/state_machine_helper.h | 2 - src/modules/gpio_led/gpio_led.c | 2 +- src/modules/mavlink/mavlink_messages.cpp | 4 +- src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/navigator/navigator_main.cpp | 4 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 25 +++--- 9 files changed, 125 insertions(+), 118 deletions(-) (limited to 'src/modules/mavlink/mavlink_receiver.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..43683f833 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[5] = "REBOOT"; arming_states_str[6] = "IN_AIR_RESTORE"; - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL_RC"; - failsafe_states_str[2] = "RTL_DL"; - failsafe_states_str[3] = "LAND"; - failsafe_states_str[4] = "TERMINATION"; - /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_MANUAL; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); } - /* start RC input check */ + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - status.failsafe_state = FAILSAFE_STATE_NORMAL; - failsafe_state_changed = true; } } @@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - - // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - // /* recover from failsafe */ - // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - // } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; - } - failsafe_state_changed = true; - } else { - mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); - } } } - /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ - if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); - } else { - /* this probably doesn't make sense since we are in mission and have global position */ - status.failsafe_state = FAILSAFE_STATE_LAND; + /* data link check */ + if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = false; + status_changed = true; } - failsafe_state_changed = true; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; - /* now set nav state according to failsafe and main state */ + /* now set navigation state according to failsafe and main state */ set_nav_state(&status); if (main_state_changed) { @@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[]) if (failsafe_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); failsafe_state_changed = false; } @@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1667,7 +1655,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - switch (status.set_nav_state) { + switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1719,8 +1707,6 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c52e618ef..df718ff99 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } } - if (ret = TRANSITION_CHANGED) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here @@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, return ret; } - /** * Check failsafe and main status and set navigation status for navigator accordingly */ void set_nav_state(struct vehicle_status_s *status) { - switch (status->failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case MAIN_STATE_MANUAL: - status->set_nav_state = NAVIGATION_STATE_MANUAL; - break; - - case MAIN_STATE_ALTCTL: - status->set_nav_state = NAVIGATION_STATE_ALTCTL; - break; + status->failsafe = false; - case MAIN_STATE_POSCTL: - status->set_nav_state = NAVIGATION_STATE_POSCTL; - break; - - case MAIN_STATE_AUTO_MISSION: - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; - break; - - case MAIN_STATE_AUTO_LOITER: - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; - break; + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost) { + status->failsafe = true; - case MAIN_STATE_AUTO_RTL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; - break; + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } + } + break; - case MAIN_STATE_ACRO: - status->set_nav_state = NAVIGATION_STATE_ACRO; - break; + case MAIN_STATE_AUTO_MISSION: + /* require data link and global position */ + if (status->data_link_lost || !status->condition_global_position_valid) { + status->failsafe = true; - default: - break; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; - case FAILSAFE_STATE_RC_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; - break; + case MAIN_STATE_AUTO_LOITER: + /* require data link and local position */ + if (status->data_link_lost || !status->condition_local_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_DL_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } break; - case FAILSAFE_STATE_LAND: - status->set_nav_state = NAVIGATION_STATE_LAND; - break; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_TERMINATION: - status->set_nav_state = NAVIGATION_STATE_TERMINATION; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } break; default: break; } + + if (status->failsafe) { + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 594bf23a1..062271764 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,8 +63,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); void set_nav_state(struct vehicle_status_s *status); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..839a7890b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b58fb4cb8..d7f56b63d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,7 +118,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->set_nav_state) { + switch (status->nav_state) { case NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED @@ -161,8 +161,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..ab3dc81c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -138,6 +139,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..70da5393f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -336,7 +336,7 @@ Navigator::task_main() } /* Do stuff according to navigation state set by commander */ - switch (_vstatus.set_nav_state) { + switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: @@ -351,8 +351,6 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 53cd6a797..19872bbd0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9390ff717..898c176fb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,7 +59,9 @@ * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, @@ -89,15 +91,9 @@ typedef enum { HIL_STATE_ON } hil_state_t; -typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ - FAILSAFE_STATE_LAND, /**< Land as safe as possible */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX, -} failsafe_state_t; - +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ NAVIGATION_STATE_ACRO, /**< Acro mode */ @@ -106,9 +102,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; @@ -171,10 +166,10 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + bool failsafe; /**< true if system is in failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -199,6 +194,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From 94e004955df3f467b7e67b3fac0d968b9a68e091 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 09:33:28 +0200 Subject: mavlink: publish telemtry status without radio status --- src/modules/mavlink/mavlink_receiver.cpp | 22 ++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 23 insertions(+) (limited to 'src/modules/mavlink/mavlink_receiver.h') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 32c5e51dd..7a6922bfa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -107,6 +107,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rc_pub(-1), _manual_pub(-1), _telemetry_heartbeat_time(0), + _radio_status_available(false), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -432,6 +433,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } + + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } void @@ -467,6 +471,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { _telemetry_heartbeat_time = hrt_absolute_time(); } + + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ab3dc81c6..cd1dab365 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -140,6 +140,7 @@ private: orb_advert_t _rc_pub; orb_advert_t _manual_pub; hrt_abstime _telemetry_heartbeat_time; + bool _radio_status_available; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; -- cgit v1.2.3