fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 15:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_rewrite' into navigator_rewrite_drton

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-29 14:10:04 +02:00
parent 0bf9c2a9b2 5db51bd704
commit 8a442c2125
31 changed files with 973 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.usb
+1 -1
View File
@@ -3,7 +3,7 @@
# USB MAVLink start
#
mavlink start -r 10000 -d /dev/ttyACM0
mavlink start -r 10000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
usleep 100000
makefiles/config_aerocore_default.mk
+1 -1
View File
@@ -80,8 +80,8 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
makefiles/config_px4fmu-v1_default.mk
+1 -1
View File
@@ -107,8 +107,8 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
makefiles/config_px4fmu-v2_default.mk
+1 -1
View File
@@ -120,8 +120,8 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
mavlink/include/mavlink/v1.0/checksum.h
+1 -1
View File
@@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
* @param data new bytes to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
{
const uint8_t *p = (const uint8_t *)pBuffer;
while (length--) {
mavlink/include/mavlink/v1.0/common/common.h
+4
View File
@@ -5,6 +5,10 @@
#ifndef COMMON_H
#define COMMON_H
#ifndef MAVLINK_H
#error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually.
#endif
#ifdef __cplusplus
extern "C" {
#endif
mavlink/include/mavlink/v1.0/mavlink_helpers.h
+14 -7
View File
@@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
#endif
{
// This code part is the same for all messages;
uint16_t checksum;
msg->magic = MAVLINK_STX;
msg->len = length;
msg->sysid = system_id;
@@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
// One sequence number per component
msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
crc_accumulate(crc_extra, &msg->checksum);
#endif
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
@@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
buf[4] = mavlink_system.compid;
buf[5] = msgid;
status->current_tx_seq++;
checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&checksum, packet, length);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
@@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
// XXX use the right sequence here
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
@@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
*/
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}
mavlink/include/mavlink/v1.0/mavlink_types.h
+2 -2
View File
@@ -28,6 +28,7 @@
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
#pragma pack(push, 1)
typedef struct param_union {
union {
float param_float;
@@ -62,13 +63,12 @@ typedef struct __mavlink_message {
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
typedef struct __mavlink_extended_message {
mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t;
#pragma pack(pop)
typedef enum {
MAVLINK_TYPE_CHAR = 0,
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
-3
View File
@@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
-3
View File
@@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0; //xxx: param
/* input conditioning */
// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+1 -6
View File
@@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
_rate_setpoint = 0.0f;
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
if(fabsf(denumerator) > FLT_EPSILON) {
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
@@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
src/lib/geo/geo.h
+1 -1
View File
@@ -50,7 +50,7 @@
__BEGIN_DECLS
#include "geo_mag_declination.h"
#include "geo_lookup/geo_mag_declination.h"
#include <stdbool.h>
src/lib/geo/module.mk
+2 -3
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# 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
@@ -35,5 +35,4 @@
# Geo library
#
SRCS = geo.c \
geo_mag_declination.c
SRCS = geo.c
src/lib/geo/geo_mag_declination.c → src/lib/geo_lookup/geo_mag_declination.c
View File
src/lib/geo/geo_mag_declination.h → src/lib/geo_lookup/geo_mag_declination.h
View File
src/lib/geo_lookup/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# 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.
#
############################################################################
#
# Geo lookup table / data library
#
SRCS = geo_mag_declination.c
MAXOPTIMIZATION = -Os
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
-13
View File
@@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
@@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");
h->yaw_off = param_find("ATT_YAW_OFF3");
h->mag_decl = param_find("ATT_MAG_DECL");
h->acc_comp = param_find("ATT_ACC_COMP");
@@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
-1
View File
@@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params {
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t roll_off, pitch_off, yaw_off;
param_t mag_decl;
param_t acc_comp;
};
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+2 -2
View File
@@ -1368,8 +1368,8 @@ FixedwingEstimator::task_main()
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = 0.0f; // XXX get form filter
_wind.covariance_east = 0.0f;
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+17 -57
View File
@@ -43,8 +43,8 @@
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Original implementation for total energy control class:
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
* Implementation for total energy control class:
* Thomas Gubler
*
* More details and acknowledgements in the referenced library headers.
*
@@ -88,7 +88,6 @@
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
@@ -199,7 +198,6 @@ private:
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
@@ -565,23 +563,6 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_pitch_damping(_parameters.pitch_damping);
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@@ -653,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
@@ -835,10 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@@ -863,9 +837,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
@@ -1228,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
@@ -1269,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
}
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
_att_sp.pitch_body = _mTecs.getPitchSetpoint();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1455,29 +1424,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
const math::Vector<3> &ground_speed,
tecs_mode mode)
{
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
}
int
src/modules/mavlink/mavlink_ftp.cpp
+415
View File
@@ -0,0 +1,415 @@
/****************************************************************************
*
* 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 <crc32.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#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);
// initialize session list
for (size_t i=0; i<kMaxSession; i++) {
_session_fds[i] = -1;
}
// drop work entries onto the free list
for (unsigned i = 0; i < kRequestQueueSize; i++) {
_qFree(&_workBufs[i]);
}
}
void
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
{
// get a free request
auto req = _dqFree();
// if we couldn't get a request slot, just drop it
if (req != nullptr) {
// decode the request
if (req->decode(mavlink, msg)) {
// and queue it for the worker
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
} else {
_qFree(req);
}
}
}
void
MavlinkFTP::_workerTrampoline(void *arg)
{
auto req = reinterpret_cast<Request *>(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->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
printf("ftp: bad crc\n");
}
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
switch (hdr->opcode) {
case kCmdNone:
break;
case kCmdTerminate:
errorCode = _workTerminate(req);
break;
case kCmdReset:
errorCode = _workReset();
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;
printf("FTP: ack\n");
} else {
printf("FTP: nak %u\n", errorCode);
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->rawData(), req->dataSize());
// then pack and send the reply back to the request source
req->reply();
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(Request *req)
{
auto hdr = req->header();
DIR *dp = opendir(req->dataAsCString());
if (dp == nullptr) {
printf("FTP: can't open path '%s'\n", req->dataAsCString());
return kErrNotDir;
}
ErrorCode errorCode = kErrNone;
struct dirent entry, *result = nullptr;
unsigned offset = 0;
// move to the requested offset
seekdir(dp, hdr->offset);
for (;;) {
// read the directory entry
if (readdir_r(dp, &entry, &result)) {
errorCode = kErrIO;
break;
}
// no more entries?
if (result == nullptr) {
if (hdr->offset != 0 && offset == 0) {
// User is requesting subsequent dir entries but there were none. This means the user asked
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break;
}
// name too big to fit?
if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
break;
}
// store the type marker
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
break;
default:
hdr->data[offset++] = kDirentUnknown;
break;
}
// copy the name, which we know will fit
strcpy((char *)&hdr->data[offset], entry.d_name);
offset += strlen(entry.d_name) + 1;
printf("FTP: list %s\n", entry.d_name);
}
closedir(dp);
hdr->size = offset;
return errorCode;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(Request *req, bool create)
{
auto hdr = req->header();
int session_index = _findUnusedSession();
if (session_index < 0) {
return kErrNoSession;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
if (fd < 0) {
return create ? kErrPerm : kErrNotFile;
}
_session_fds[session_index] = fd;
hdr->session = session_index;
hdr->size = 0;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(Request *req)
{
auto hdr = req->header();
int session_index = hdr->session;
if (!_validSession(session_index)) {
return kErrNoSession;
}
// Seek to the specified position
printf("Seek %d\n", hdr->offset);
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
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
return kErrIO;
}
printf("Read success %d\n", bytes_read);
hdr->size = bytes_read;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(Request *req)
{
#if 0
// NYI: Coming soon
auto hdr = req->header();
// look up session
auto session = getSession(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;
#else
return kErrPerm;
#endif
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemove(Request *req)
{
auto hdr = req->header();
// for now, send error reply
return kErrPerm;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(Request *req)
{
auto hdr = req->header();
if (!_validSession(hdr->session)) {
return kErrNoSession;
}
::close(_session_fds[hdr->session]);
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
}
return kErrNone;
}
bool
MavlinkFTP::_validSession(unsigned index)
{
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
return false;
}
return true;
}
int
MavlinkFTP::_findUnusedSession(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
return i;
}
}
return -1;
}
char *
MavlinkFTP::Request::dataAsCString()
{
// guarantee nul termination
if (header()->size < kMaxDataLength) {
requestData()[header()->size] = '\0';
} else {
requestData()[kMaxDataLength - 1] = '\0';
}
// and return data
return (char *)&(header()->data[0]);
}
src/modules/mavlink/mavlink_ftp.h
+226
View File
@@ -0,0 +1,226 @@
/****************************************************************************
*
* 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 <dirent.h>
#include <queue.h>
#include <nuttx/wqueue.h>
#include <systemlib/err.h>
#include "mavlink_messages.h"
class MavlinkFTP
{
public:
MavlinkFTP();
static MavlinkFTP *getServer();
// static interface
void handle_message(Mavlink* mavlink,
mavlink_message_t *msg);
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[];
};
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
};
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
{
public:
union {
dq_entry_t entry;
work_s work;
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
_mavlink = mavlink;
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
return true;
}
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_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) {
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);
}
}
uint8_t *rawData() { return &_message.data[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
uint16_t sequence() const { return _message.seqnr; }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
mavlink_encapsulated_data_t _message;
};
static const uint8_t kProtocolMagic = 'f';
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
/// 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;
}
};
src/modules/mavlink/mavlink_main.cpp
+75 -24
View File
@@ -83,6 +83,10 @@
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
#ifndef MAVLINK_CRC_EXTRA
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
#endif
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0};
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
Mavlink *instance;
switch (channel) {
@@ -198,7 +203,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
warnx("TX FAIL");
// XXX overflow perf
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
@@ -230,6 +235,7 @@ Mavlink::Mavlink() :
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
@@ -459,7 +465,7 @@ Mavlink::get_instance_id()
return _instance_id;
}
mavlink_channel_t
const mavlink_channel_t
Mavlink::get_channel()
{
return _channel;
@@ -537,6 +543,16 @@ void Mavlink::mavlink_update_system(void)
_use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::get_system_id()
{
return mavlink_system.sysid;
}
int Mavlink::get_component_id()
{
return mavlink_system.compid;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
/* process baud rate */
@@ -1631,11 +1647,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
int
Mavlink::message_buffer_init(int size)
{
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
return (_message_buffer.data == 0) ? ERROR : OK;
int ret;
if (_message_buffer.data == 0) {
ret = ERROR;
_message_buffer.size = 0;
} else {
ret = OK;
}
return ret;
}
void
@@ -1763,7 +1789,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1819,6 +1845,10 @@ Mavlink::task_main(int argc, char *argv[])
_wait_to_transmit = true;
break;
case 'x':
_ftp_on = true;
break;
default:
err_flag = true;
break;
@@ -1884,9 +1914,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on) {
/* initialize message buffer if multiplexing is on */
if (OK != message_buffer_init(300)) {
if (_passing_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP.
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -2049,32 +2082,50 @@ Mavlink::task_main(int argc, char *argv[])
}
}
/* pass messages from other UARTs */
if (_passing_on) {
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
bool is_part;
void *read_ptr;
uint8_t *read_ptr;
uint8_t *write_ptr;
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr(&read_ptr, &is_part);
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
/* write first part of buffer */
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t*)&msg;
// Pull a single message from the buffer
int read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}
@@ -2124,7 +2175,7 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
if (_passing_on) {
if (_passing_on || _ftp_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
@@ -2266,7 +2317,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
}
int mavlink_main(int argc, char *argv[])
src/modules/mavlink/mavlink_main.h
+96 -76
View File
@@ -123,27 +123,41 @@ public:
/**
* Display the mavlink status.
*/
void status();
void status();
static int stream(int argc, char *argv[]);
static int stream(int argc, char *argv[]);
static int instance_count();
static int instance_count();
static Mavlink *new_instance();
static Mavlink *new_instance();
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance_for_device(const char *device_name);
static Mavlink *get_instance_for_device(const char *device_name);
static int destroy_all_instances();
static int destroy_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
static int get_uart_fd(unsigned index);
int get_uart_fd();
int get_uart_fd();
/**
* Get the MAVLink system id.
*
* @return The system ID of this vehicle
*/
int get_system_id();
/**
* Get the MAVLink component id.
*
* @return The component ID of this vehicle
*/
int get_component_id();
const char *_device_name;
@@ -153,30 +167,30 @@ public:
MAVLINK_MODE_CAMERA
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
bool get_forwarding_on() { return _forwarding_on; }
/**
* Handle waypoint related messages.
*/
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
static int start_helper(int argc, char *argv[]);
static int start_helper(int argc, char *argv[]);
/**
* Handle parameter related messages.
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Enable / disable Hardware in the Loop simulation mode.
@@ -186,90 +200,96 @@ public:
* requested change could not be made or was
* redundant.
*/
int set_hil_enabled(bool hil_enabled);
int set_hil_enabled(bool hil_enabled);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
int get_instance_id();
int get_instance_id();
/**
* Enable / disable hardware flow control.
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
int enable_flow_control(bool enabled);
mavlink_channel_t get_channel();
const mavlink_channel_t get_channel();
bool _task_should_exit; /**< if true, mavlink task should exit */
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
int get_mavlink_fd() { return _mavlink_fd; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
protected:
Mavlink *next;
Mavlink *next;
private:
int _instance_id;
int _instance_id;
int _mavlink_fd;
bool _task_running;
int _mavlink_fd;
bool _task_running;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
orb_advert_t _mission_pub;
struct mission_s mission;
MAVLINK_MODE _mode;
orb_advert_t _mission_pub;
struct mission_s mission;
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
unsigned int _total_counter;
pthread_t _receive_thread;
pthread_t _receive_thread;
/* Allocate storage space for waypoints */
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
int _uart_fd;
int _baudrate;
int _datarate;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate;
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
* to len - 1, the end of the param list.
*/
unsigned int _mavlink_param_queue_index;
unsigned int _mavlink_param_queue_index;
bool mavlink_link_termination_allowed;
bool mavlink_link_termination_allowed;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
bool _flow_control_enabled;
struct mavlink_message_buffer {
int write_ptr;
@@ -277,11 +297,13 @@ private:
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf; /**< loop performance counter */
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
@@ -294,7 +316,7 @@ private:
* @param param The parameter id to send.
* @return zero on success, nonzero on failure.
*/
int mavlink_pm_send_param(param_t param);
int mavlink_pm_send_param(param_t param);
/**
* Send one parameter identified by index.
@@ -302,7 +324,7 @@ private:
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
@@ -310,14 +332,14 @@ private:
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
@@ -327,12 +349,12 @@ private:
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_pm_start_queued_send();
void mavlink_update_system();
void mavlink_update_system();
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
@@ -359,8 +381,6 @@ private:
int message_buffer_is_empty();
bool message_buffer_write(void *ptr, int size);
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
src/modules/mavlink/mavlink_messages.cpp
+9 -2
View File
@@ -254,8 +254,15 @@ protected:
struct position_setpoint_triplet_s pos_sp_triplet;
/* always send the heartbeat, independent of the update status of the topics */
(void)status_sub->update(&status);
(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
if (!status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
/* if topic update failed fill it with defaults */
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
src/modules/mavlink/mavlink_receiver.cpp
+7
View File
@@ -114,6 +114,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()
@@ -156,6 +159,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_heartbeat(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
default:
break;
}
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -68,6 +68,8 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include "mavlink_ftp.h"
class Mavlink;
class MavlinkReceiver
src/modules/mavlink/module.mk
+2 -1
View File
@@ -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
src/modules/sensors/sensor_params.c
+30
View File
@@ -242,6 +242,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
*/
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
/**
* Board rotation Y (Pitch) offset
*
* This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
/**
* Board rotation X (Roll) offset
*
* This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
/**
* Board rotation Z (YAW) offset
*
* This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
/**
* External magnetometer rotation
*
src/modules/sensors/sensors.cpp
+22 -1
View File
@@ -229,7 +229,7 @@ private:
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@@ -252,6 +252,8 @@ private:
int board_rotation;
int external_mag_rotation;
float board_offset[3];
int rc_map_roll;
int rc_map_pitch;
@@ -341,6 +343,8 @@ private:
param_t board_rotation;
param_t external_mag_rotation;
param_t board_offset[3];
} _parameter_handles; /**< handles for interesting parameters */
@@ -587,6 +591,11 @@ Sensors::Sensors() :
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
/* rotation offsets */
_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* fetch initial parameter values */
parameters_update();
@@ -791,6 +800,18 @@ Sensors::parameters_update()
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
/** fine tune board offset on parameter update **/
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
return OK;
}
src/modules/systemlib/perf_counter.h
+1 -1
View File
@@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle);
/**
* Count a performance event.
*
* This call only affects counters that take single events; PC_COUNT etc.
* This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
*
* @param handle The handle returned from perf_alloc.
*/