From 88adeee6695a2160f9eac130d3923129728dd008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 13:35:14 +0200 Subject: [PATCH 1/6] define force setpoint uorb topic --- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vehicle_force_setpoint.h | 65 +++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_force_setpoint.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e9..d453142222 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,6 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/vehicle_force_setpoint.h" +ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h new file mode 100644 index 0000000000..c7b5f558e1 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * 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 vehicle_local_force_setpoint.h + * @author Thomas Gubler + * Definition of force (NED) setpoint uORB topic. Typically this can be used + * by a position control app togeth with an attitude control app. + */ + +#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ +#define TOPIC_VEHICLE_FORCE_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_force_setpoint_s { + float x; /**< in N NED */ + float y; /**< in N NED */ + float z; /**< in N NED */ +}; /**< Desired force in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_force_setpoint); + +#endif From 3ddb502d2e1b07705bfaa59c89528d53d0444303 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 14:14:13 +0200 Subject: [PATCH 2/6] force setpoint: fix comment --- src/modules/uORB/topics/vehicle_force_setpoint.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h index c7b5f558e1..cd948985ff 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,10 +32,10 @@ ****************************************************************************/ /** - * @file vehicle_local_force_setpoint.h + * @file vehicle_force_setpoint.h * @author Thomas Gubler * Definition of force (NED) setpoint uORB topic. Typically this can be used - * by a position control app togeth with an attitude control app. + * by a position control app together with an attitude control app. */ #ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ From e8c34fa174c6a47d5d2def2417c58beb846bb30f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 10:25:57 +0200 Subject: [PATCH 3/6] force setpoint uorb topic: add yaw field --- src/modules/uORB/topics/vehicle_force_setpoint.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h index cd948985ff..e3a7360b2b 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -52,6 +52,7 @@ struct vehicle_force_setpoint_s { float x; /**< in N NED */ float y; /**< in N NED */ float z; /**< in N NED */ + float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */ }; /**< Desired force in NED frame */ /** From 9f1661f1f8808b7c83f73284cee9a1e9279165d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:58:08 +0200 Subject: [PATCH 4/6] Silence FTP transactions, be vocal about errors --- src/modules/mavlink/mavlink_ftp.cpp | 19 +++++++++-------- src/modules/mavlink/mavlink_ftp.h | 27 ++++++++++++------------ src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 57ae6076c5..675a6870e1 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -117,10 +117,10 @@ MavlinkFTP::_worker(Request *req) if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; - printf("ftp: bad crc\n"); + warnx("ftp: bad crc"); } - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); switch (hdr->opcode) { case kCmdNone: @@ -167,9 +167,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - printf("FTP: ack\n"); + //warnx("FTP: ack\n"); } else { - printf("FTP: nak %u\n", errorCode); + warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; hdr->size = 1; hdr->data[0] = errorCode; @@ -206,11 +206,11 @@ MavlinkFTP::_workList(Request *req) DIR *dp = opendir(dirPath); if (dp == nullptr) { - printf("FTP: can't open path '%s'\n", dirPath); + warnx("FTP: can't open path '%s'", dirPath); return kErrNotDir; } - printf("FTP: list %s offset %d\n", dirPath, hdr->offset); + //warnx("FTP: list %s offset %d", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -222,7 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - printf("FTP: list %s readdir_r failure\n", dirPath); + warnx("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -304,19 +304,20 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - printf("Seek %d\n", hdr->offset); + //warnx("seek %d", hdr->offset); if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location + warnx("seek fail"); 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 + warnx("read fail %d", bytes_read); return kErrIO; } - printf("Read success %d\n", bytes_read); hdr->size = bytes_read; return kErrNone; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index e22e615538..59237a4c4e 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -147,20 +147,21 @@ private: 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) { + _mavlink->lockMessageBufferMutex(); + bool success = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!success) { warnx("FTP TX ERR"); - } else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } + } + // 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]; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 81966caf80..4848866823 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -442,7 +442,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", manual.x, manual.y, manual.r, manual.z); + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); From 86932423af88f9ed0a53e0fab6b12160ff0b3efb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 10:50:59 +0200 Subject: [PATCH 5/6] update mavlink to latest master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 45a71d6564..3711190d23 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 45a71d6564bc5c47ed97d620089e17ca48bab73f +Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 From 681d746e060cbd82008967ea56dc950c9ca7ef49 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:21:08 +0200 Subject: [PATCH 6/6] update mavlink to latest master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 3711190d23..d1ebe85eb6 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 +Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628