forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into sensor_drivers
This commit is contained in:
commit
2651327205
|
@ -1 +1 @@
|
|||
Subproject commit 45a71d6564bc5c47ed97d620089e17ca48bab73f
|
||||
Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50
|
|
@ -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;
|
||||
|
|
|
@ -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]; }
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -206,6 +206,9 @@ 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);
|
||||
|
||||
#include "topics/tecs_status.h"
|
||||
ORB_DEFINE(tecs_status, struct tecs_status_s);
|
||||
|
||||
|
|
|
@ -0,0 +1,65 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 vehicle_force_setpoint.h
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* Definition of force (NED) setpoint uORB topic. Typically this can be used
|
||||
* by a position control app together 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 */
|
||||
float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */
|
||||
}; /**< Desired force in NED frame */
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vehicle_force_setpoint);
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue