Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/obcfailsafe' into swissfang

Browse Source 
Conflicts:
	src/lib/external_lgpl/tecs/tecs.cpp
	src/modules/commander/commander_params.c
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Thomas Gubler
2014-09-30 15:38:27 +02:00
parent 15eee418a0 ab400089bc
commit cc05f0f185
30 changed files with 1595 additions and 338 deletions
Download Patch File Download Diff File Expand all files Collapse all files
LICENSE.md
+41
View File
@@ -0,0 +1,41 @@
The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
to be made under the same license. Any exception to this general rule is listed below.
/****************************************************************************
*
* Copyright (c) 2012-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.
*
****************************************************************************/
- PX4 middleware: BSD 3-clause
- PX4 flight control stack: BSD 3-clause
- NuttX operating system: BSD 3-clause
- Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation.
README.md
+10
View File
@@ -0,0 +1,10 @@
## PX4 Aerial Middleware and Flight Control Stack ##
* Official Website: http://px4.io
* License: BSD 3-clause (see LICENSE.md)
* Supported airframes:
* Multicopters
* Fixed wing
* Binaries (always up-to-date from master):
* [Downloads](https://pixhawk.org/downloads)
* Mailing list: [Google Groups](http://groups.google.com/group/px4users)
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore
View File
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data
BIN
View File
Binary file not shown.
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data
BIN
View File
Binary file not shown.
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data
BIN
View File
Binary file not shown.
makefiles/config_px4fmu-v1_default.mk
-3
View File
@@ -27,8 +27,6 @@ MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
@@ -42,7 +40,6 @@ MODULES += modules/sensors
# System commands
#
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
makefiles/config_px4fmu-v2_test.mk
+3
View File
@@ -54,6 +54,9 @@ MODULES += lib/conversion
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += modules/unit_test
MODULES += modules/mavlink/mavlink_tests
#
# Transitional support - add commands from the NuttX export archive.
#
src/lib/external_lgpl/tecs/tecs.cpp
+1
View File
@@ -238,6 +238,7 @@ void TECS::_update_height_demand(float demand, float state)
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
_hgt_dem_adj_last = _hgt_dem_adj;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
src/modules/commander/commander_params.c
+1 -1
View File
@@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
* @min 0.0f
* @max 7.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f);
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
/** RC loss time threshold
*
src/modules/mavlink/mavlink_ftp.cpp
+306 -144
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_ftp.h
+109 -173
View File
@@ -33,17 +33,8 @@
#pragma once
/**
* @file mavlink_ftp.h
*
* MAVLink remote file server.
*
* 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.
*
*/
/// @file mavlink_ftp.h
/// @author px4dev, Don Gagne <don@thegagnes.com>
#include <dirent.h>
#include <queue.h>
@@ -54,183 +45,128 @@
#include "mavlink_messages.h"
#include "mavlink_main.h"
class MavlinkFtpTest;
/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
class MavlinkFTP
{
public:
/// @brief Returns the one Mavlink FTP server in the system.
static MavlinkFTP* get_server(void);
/// @brief Contructor is only public so unit test code can new objects.
MavlinkFTP();
/// @brief Adds the specified message to the work queue.
void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
/// @brief Sets up the server to run in unit test mode.
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
/// @param ftp_test MavlinkFtpTest object which the function is associated with
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
static MavlinkFTP *getServer();
// static interface
void handle_message(Mavlink* mavlink,
mavlink_message_t *msg);
private:
static const unsigned kRequestQueueSize = 2;
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
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
/// 32 bit alignment to avoid usage of any pack pragmas.
struct PayloadHeader
{
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[];
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 req_opcode; ///< Request opcode returned in kRspAck, kRspNak message
uint8_t padding[2]; ///< 32 bit aligment padding
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[]; ///< command data, varies by Opcode
};
/// @brief Command opcodes
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
kCmdNone, ///< ignored, always acked
kCmdTerminateSession, ///< Terminates open Read session
kCmdResetSessions, ///< Terminates all open Read sessions
kCmdListDirectory, ///< List files in <path> from <offset>
kCmdOpenFile, ///< Opens file at <path> for reading, returns <session>
kCmdReadFile, ///< Reads <size> bytes from <offset> in <session>
kCmdCreateFile, ///< Creates file at <path> for writing, returns <session>
kCmdWriteFile, ///< Appends <size> bytes to file in <session>
kCmdRemoveFile, ///< Remove file at <path>
kCmdCreateDirectory, ///< Creates directory at <path>
kCmdRemoveDirectory, ///< Removes Directory at <path>, must be empty
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
};
/// @brief Error codes returned in Nak response PayloadHeader.data[0].
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
kErrFail, ///< Unknown failure
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
kErrInvalidDataSize, ///< PayloadHeader.size is invalid
kErrInvalidSession, ///< Session is not currently open
kErrNoSessionsAvailable, ///< All available Sessions in use
kErrEOF, ///< Offset past end of file for List and Read commands
kErrUnknownCommand ///< Unknown command opcode
};
private:
/// @brief Unit of work which is queued to work_queue
struct Request
{
public:
union {
dq_entry_t entry;
work_s work;
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
_systemId = fromMessage->sysid;
_mavlink = mavlink;
mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
return _message.target_system == _mavlink->get_system_id();
}
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_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);
_mavlink->unlockMessageBufferMutex();
if (!success) {
warnx("FTP TX ERR");
}
#ifdef MAVLINK_FTP_DEBUG
else {
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
_mavlink->get_system_id(),
_mavlink->get_component_id(),
_mavlink->get_channel(),
len,
msg.checksum);
}
#endif
}
uint8_t *rawData() { return &_message.payload[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
mavlink_file_transfer_protocol_t _message;
uint8_t _systemId;
work_s work; ///< work queue entry
Mavlink *mavlink; ///< Mavlink to reply to
uint8_t serverSystemId; ///< System ID to send from
uint8_t serverComponentId; ///< Component ID to send from
uint8_t serverChannel; ///< Channel to send to
uint8_t targetSystemId; ///< System ID to target reply to
mavlink_file_transfer_protocol_t message; ///< Protocol message
};
Request *_get_request(void);
void _return_request(Request *req);
void _lock_request_queue(void);
void _unlock_request_queue(void);
char *_data_as_cstring(PayloadHeader* payload);
static void _worker_trampoline(void *arg);
void _process_request(Request *req);
void _reply(Request *req);
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workOpen(PayloadHeader *payload, bool create);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader* payload);
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
ErrorCode _workCreateDirectory(PayloadHeader *payload);
ErrorCode _workRemoveFile(PayloadHeader *payload);
/// 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;
}
static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests
Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work
dq_queue_t _request_queue; ///< Queue of available Request buffers
sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue
int _find_unused_session(void);
bool _valid_session(unsigned index);
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
/// @brief Maximum data size in RequestHeader::data
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
static const unsigned kMaxSession = 2; ///< Max number of active sessions
int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc;
};
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -123,7 +123,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
{
// make sure the FTP server is started
(void)MavlinkFTP::getServer();
(void)MavlinkFTP::get_server();
}
MavlinkReceiver::~MavlinkReceiver()
@@ -175,7 +175,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
break;
default:
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+757
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+107
View File
@@ -0,0 +1,107 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/// @file mavlink_ftp_test.h
/// @author Don Gagne <don@thegagnes.com>
#pragma once
#include <unit_test/unit_test.h>
#include "../mavlink_bridge_header.h"
#include "../mavlink_ftp.h"
class MavlinkFtpTest : public UnitTest
{
public:
MavlinkFtpTest();
virtual ~MavlinkFtpTest();
virtual void init(void);
virtual void cleanup(void);
virtual void runTests(void);
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
static const uint8_t serverSystemId = 50; ///< System ID for server
static const uint8_t serverComponentId = 1; ///< Component ID for server
static const uint8_t serverChannel = 0; ///< Channel to send to
static const uint8_t clientSystemId = 1; ///< System ID for client
static const uint8_t clientComponentId = 0; ///< Component ID for client
// We don't want any of these
MavlinkFtpTest(const MavlinkFtpTest&);
MavlinkFtpTest& operator=(const MavlinkFtpTest&);
private:
bool _ack_test(void);
bool _bad_opcode_test(void);
bool _bad_datasize_test(void);
bool _list_test(void);
bool _list_eof_test(void);
bool _open_badfile_test(void);
bool _open_terminate_test(void);
bool _terminate_badsession_test(void);
bool _read_test(void);
bool _read_badsession_test(void);
bool _removedirectory_test(void);
bool _createdirectory_test(void);
bool _removefile_test(void);
void _receive_message(const mavlink_message_t *msg);
void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header,
uint8_t size,
const uint8_t *data,
mavlink_file_transfer_protocol_t *ftp_msg_reply,
MavlinkFTP::PayloadHeader **payload_reply);
void _cleanup_microsd(void);
MavlinkFTP *_ftp_server;
mavlink_message_t _reply_msg;
uint16_t _lastOutgoingSeqNumber;
struct ReadTestCase {
const char *file;
const uint16_t length;
};
static const ReadTestCase _rgReadTestCases[];
static const char _unittest_microsd_dir[];
static const char _unittest_microsd_file[];
};
src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py
+7
View File
@@ -0,0 +1,7 @@
import sys
print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2]
file = open(sys.argv[1], 'w')
for i in range(int(sys.argv[2])):
b = bytearray([i & 0xFF])
file.write(b)
file.close()
src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
+52
View File
@@ -0,0 +1,52 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file mavlink_ftp_tests.cpp
*/
#include <systemlib/err.h>
#include "mavlink_ftp_test.h"
extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
int mavlink_tests_main(int argc, char *argv[])
{
MavlinkFtpTest* test = new MavlinkFtpTest;
test->runTests();
test->printResults();
return 0;
}
src/modules/mavlink/mavlink_tests/module.mk
+48
View File
@@ -0,0 +1,48 @@
############################################################################
#
# 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.
#
############################################################################
#
# System state machine tests.
#
MODULE_COMMAND = mavlink_tests
SRCS = mavlink_tests.cpp \
mavlink_ftp_test.cpp \
../mavlink_ftp.cpp \
../mavlink.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 5000
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST
src/modules/navigator/navigator_main.cpp
+7 -5
View File
@@ -87,6 +87,7 @@
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
#define GEOFENCE_CHECK_INTERVAL 200000
namespace navigator
{
@@ -343,7 +344,7 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
bool have_geofence_position_data = false;
static bool have_geofence_position_data = false;
/* gps updated */
if (fds[7].revents & POLLIN) {
@@ -388,17 +389,18 @@ Navigator::task_main()
if (fds[0].revents & POLLIN) {
global_position_update();
static int gposcounter = 0;
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS &&
gposcounter % 10 == 0) {
/* Geofence is checked only every 10th gpos update */
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
gposcounter++;
}
/* Check geofence violation */
if (have_geofence_position_data) {
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
if (!inside) {
/* inform other apps via the mission result */
_mission_result.geofence_violated = true;
src/modules/systemlib/circuit_breaker.c
+15 -1
View File
@@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
* @max 121212
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
/**
* Circuit breaker for engine failure detection
@@ -122,6 +122,20 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0);
/**
* Circuit breaker for gps failure detection
*
* Setting this parameter to 240024 will disable the gps failure detection.
* If the aircraft is in gps failure mode the gps failure flag will be
* set to healthy
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
src/modules/systemlib/mixer/mixer.h
+6
View File
@@ -130,6 +130,9 @@
#include <nuttx/config.h>
#include "drivers/drv_mixer.h"
#include <uORB/topics/multirotor_motor_limits.h>
#include "mixer_load.h"
/**
@@ -531,6 +534,9 @@ private:
float _yaw_scale;
float _idle_speed;
orb_advert_t _limits_pub;
multirotor_motor_limits_s _limits;
unsigned _rotor_count;
const Rotor *_rotors;
src/modules/systemlib/mixer/mixer_multirotor.cpp
+21 -1
View File
@@ -36,7 +36,8 @@
*
* Multi-rotor mixers.
*/
#include <uORB/uORB.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <nuttx/config.h>
#include <sys/types.h>
@@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float min_out = 0.0f;
float max_out = 0.0f;
_limits.roll_pitch = false;
_limits.yaw = false;
_limits.throttle_upper = false;
_limits.throttle_lower = false;
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +
@@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* limit yaw if it causes outputs clipping */
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
yaw = -out / _rotors[i].yaw_scale;
_limits.yaw = true;
}
/* calculate min and max output values */
@@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
}
_limits.roll_pitch = true;
} else {
/* roll/pitch mixed without limiting, add yaw control */
@@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
_limits.throttle_upper = true;
} else {
scale_out = 1.0f;
@@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* scale outputs to range _idle_speed..1, and do final limiting */
for (unsigned i = 0; i < _rotor_count; i++) {
if (outputs[i] < _idle_speed) {
_limits.throttle_lower = true;
}
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
/* publish/advertise motor limits if running on FMU */
if (_limits_pub > 0) {
orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
} else {
_limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
}
#endif
return _rotor_count;
}
src/modules/systemlib/param/param.c
+2 -1
View File
@@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
v = &param_info_base[param].val;
}
if (param_type(param) == PARAM_TYPE_STRUCT) {
if (param_type(param) >= PARAM_TYPE_STRUCT
&& param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
result = v->p;
} else {
src/modules/systemlib/param/param.h
+1 -1
View File
@@ -307,7 +307,7 @@ __EXPORT int param_load_default(void);
struct param_info_s __param__##_name = { \
#_name, \
PARAM_TYPE_STRUCT + sizeof(_default), \
.val.p = &_default; \
.val.p = &_default \
}
/**
src/modules/uORB/objects_common.cpp
+3
View File
@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
#include "topics/multirotor_motor_limits.h"
ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
#include "topics/telemetry_status.h"
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
src/modules/uORB/topics/multirotor_motor_limits.h
+69
View File
@@ -0,0 +1,69 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 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.
*
****************************************************************************/
/**
* @file multirotor_motor_limits.h
*
* Definition of multirotor_motor_limits topic
*/
#ifndef MULTIROTOR_MOTOR_LIMITS_H_
#define MULTIROTOR_MOTOR_LIMITS_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* Motor limits
*/
struct multirotor_motor_limits_s {
uint8_t roll_pitch : 1; // roll/pitch limit reached
uint8_t yaw : 1; // yaw limit reached
uint8_t throttle_lower : 1; // lower throttle limit reached
uint8_t throttle_upper : 1; // upper throttle limit reached
uint8_t reserved : 4;
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(multirotor_motor_limits);
#endif
src/modules/uavcan/sensors/gnss.cpp
+3 -5
View File
@@ -43,8 +43,6 @@
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
@@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
auto report = ::vehicle_gps_position_s();
report.timestamp_position = hrt_absolute_time();
report.lat = msg.lat_1e7;
report.lon = msg.lon_1e7;
report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
report.timestamp_variance = report.timestamp_position;
src/modules/unit_test/unit_test.cpp
+5
View File
@@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file,
{
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
}
void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
{
warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
}
src/modules/unit_test/unit_test.h
+18
View File
@@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test)
UnitTest();
virtual ~UnitTest();
virtual void init(void) { };
virtual void cleanup(void) { };
virtual void runTests(void) = 0;
void printResults(void);
void printAssert(const char* msg, const char* test, const char* file, int line);
void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
#define ut_assert(message, test) \
do { \
@@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test)
} \
} while (0)
#define ut_compare(message, v1, v2) \
do { \
int _v1 = v1; \
int _v2 = v2; \
if (_v1 != _v2) { \
printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
return false; \
} else { \
mu_assertion()++; \
} \
} while (0)
#define ut_run_test(test) \
do { \
warnx("RUNNING TEST: %s", #test); \
mu_tests_run()++; \
init(); \
if (!test()) { \
warnx("TEST FAILED: %s", #test); \
mu_tests_failed()++; \
@@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test)
warnx("TEST PASSED: %s", #test); \
mu_tests_passed()++; \
} \
cleanup(); \
} while (0)
};
uavcan
+1 -1
Submodule uavcan updated: c4c14c60fb...286adbcc56