fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 12:08:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into beta

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-06 19:40:15 +02:00
parent b9dc03da92 0d1d92484b
commit d9d0582777
35 changed files with 1474 additions and 474 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+7
View File
@@ -28,3 +28,10 @@ set MIXER quad_w
set PWM_OUT 1234
set PWM_MIN 1200
set MIXER_AUX pass
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
ROMFS/px4fmu_common/init.d/rc.mc_apps
+13 -3
View File
@@ -4,11 +4,21 @@
# att & pos estimator, att & pos control.
#
# previously (2014) the system was relying on
# INAV, which defaults to 0 now.
# The system is defaulting to INAV_ENABLED = 1
# but users can alternatively try the EKF-based
# filter by setting INAV_ENABLED = 0
if param compare INAV_ENABLED 1
then
attitude_estimator_ekf start
# The system is defaulting to EKF_ATT_ENABLED = 1
# and uses the older EKF filter. However users can
# enable the new quaternion based complimentary
# filter by setting EKF_ATT_ENABLED = 0.
if param compare EKF_ATT_ENABLED 1
then
attitude_estimator_ekf start
else
attitude_estimator_q start
fi
position_estimator_inav start
else
ekf_att_pos_estimator start
ROMFS/px4fmu_common/mixers/pass.aux.mix
+21
View File
@@ -0,0 +1,21 @@
# Manual pass through mixer for servo outputs 1-4
# AUX1 channel (select RC channel with RC_MAP_AUX1 param)
M: 1
O: 10000 10000 0 -10000 10000
S: 3 5 10000 10000 0 -10000 10000
# AUX2 channel (select RC channel with RC_MAP_AUX2 param)
M: 1
O: 10000 10000 0 -10000 10000
S: 3 6 10000 10000 0 -10000 10000
# AUX3 channel (select RC channel with RC_MAP_AUX3 param)
M: 1
O: 10000 10000 0 -10000 10000
S: 3 7 10000 10000 0 -10000 10000
# FLAPS channel (select RC channel with RC_MAP_FLAPS param)
M: 1
O: 10000 10000 0 -10000 10000
S: 3 4 10000 10000 0 -10000 10000
Tools/px4params/srcparser.py
+7 -3
View File
@@ -57,10 +57,10 @@ class Parameter(object):
def GetType(self):
return self.type
def GetDefault(self):
return self.default
def SetField(self, code, value):
"""
Set named field value
@@ -80,6 +80,10 @@ class Parameter(object):
"""
Return value of the given field code or None if not found.
"""
fv = self.fields.get(code)
if not fv:
# required because python 3 sorted does not accept None
return ""
return self.fields.get(code)
class SourceParser(object):
@@ -89,7 +93,7 @@ class SourceParser(object):
re_split_lines = re.compile(r'[\r\n]+')
re_comment_start = re.compile(r'^\/\*\*')
re_comment_content = re.compile(r'^\*\s*(.*)')
re_comment_content = re.compile(r'^\*\s*(.*)')
re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
re_comment_end = re.compile(r'(.*?)\s*\*\/')
re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
Tools/px4params/xmlout.py
+1 -2
View File
@@ -52,5 +52,4 @@ class XMLOutput():
self.xml_document = ET.ElementTree(xml_parameters)
def Save(self, filename):
with codecs.open(filename, 'w', 'utf-8') as f:
self.xml_document.write(f)
self.xml_document.write(filename, encoding="UTF-8")
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -77,6 +77,7 @@ MODULES += modules/land_detector
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_q
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
makefiles/firmware.mk
+1 -1
View File
@@ -494,7 +494,7 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
$(PRODUCT_BUNDLE): $(PRODUCT_BIN)
@$(ECHO) %% Generating $@
ifdef GEN_PARAM_XML
python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml
$(Q) $(PYTHON) $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml
$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
--git_identity $(PX4_BASE) \
--parameter_xml $(PRODUCT_PARAMXML) \
src/drivers/gimbal/gimbal.cpp
+3 -3
View File
@@ -306,17 +306,17 @@ Gimbal::cycle()
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
if (_attitude_compensation_roll) {
roll = -att.roll;
roll = 1.0f / M_PI_F * -att.roll;
updated = true;
}
if (_attitude_compensation_pitch) {
pitch = -att.pitch;
pitch = 1.0f / M_PI_F * -att.pitch;
updated = true;
}
if (_attitude_compensation_yaw) {
yaw = att.yaw;
yaw = 1.0f / M_PI_F * att.yaw;
updated = true;
}
src/drivers/px4io/px4io.cpp
+22 -16
View File
@@ -1171,15 +1171,27 @@ PX4IO::io_set_control_state(unsigned group)
if (!changed && (!_in_esc_calibration_mode || group != 0)) {
return -1;
}
else if (_in_esc_calibration_mode && group == 0) {
// modify controls to get max pwm (full thrust) on every esc
} else if (_in_esc_calibration_mode && group == 0) {
/* modify controls to get max pwm (full thrust) on every esc */
memset(&controls, 0, sizeof(controls));
controls.control[3] = 1.0f; // set maximum thrust
/* set maximum thrust */
controls.control[3] = 1.0f;
}
for (unsigned i = 0; i < _max_controls; i++) {
regs[i] = FLOAT_TO_REG(controls.control[i]);
/* ensure FLOAT_TO_REG does not produce an integer overflow */
float ctrl = controls.control[i];
if (ctrl < -1.0f) {
ctrl = -1.0f;
} else if (ctrl > 1.0f) {
ctrl = 1.0f;
}
regs[i] = FLOAT_TO_REG(ctrl);
}
/* copy values to registers in IO */
@@ -1731,20 +1743,14 @@ PX4IO::io_publish_pwm_outputs()
uint16_t ctl[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
if (ret != OK){
if (ret != OK)
return ret;
}
unsigned maxouts = sizeof(outputs.output) / sizeof(outputs.output[0]);
unsigned actuator_max = (_max_actuators > maxouts) ? maxouts : _max_actuators;
/* convert from register format to float */
for (unsigned i = 0; i < actuator_max; i++){
for (unsigned i = 0; i < _max_actuators; i++)
outputs.output[i] = ctl[i];
}
outputs.noutputs = actuator_max;
outputs.noutputs = _max_actuators;
/* lazily advertise on first publication */
if (_to_outputs == 0) {
@@ -2075,13 +2081,13 @@ PX4IO::print_status(bool extended_status)
printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
}
printf("actuators (including S.BUS)");
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
printf("\n");
printf("hardware servo ports");
printf("servos");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));
src/lib/mathlib/math/Matrix.hpp
+18
View File
@@ -135,6 +135,24 @@ public:
}
#endif
/**
* set row from vector
*/
void set_row(unsigned int row, const Vector<N> v) {
for (unsigned i = 0; i < N; i++) {
data[row][i] = v.data[i];
}
}
/**
* set column from vector
*/
void set_col(unsigned int col, const Vector<M> v) {
for (unsigned i = 0; i < M; i++) {
data[i][col] = v.data[i];
}
}
/**
* access by index
*/
src/lib/mathlib/math/Quaternion.hpp
+87 -29
View File
@@ -93,6 +93,19 @@ public:
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
}
/**
* division
*/
Quaternion operator /(const Quaternion &q) const {
float norm = q.length_squared();
return Quaternion(
( data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
(- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
(- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
(- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
);
}
/**
* derivative
*/
@@ -108,6 +121,69 @@ public:
return Q * v * 0.5f;
}
/**
* conjugate
*/
Quaternion conjugated() const {
return Quaternion(data[0], -data[1], -data[2], -data[3]);
}
/**
* inversed
*/
Quaternion inversed() const {
float norm = length_squared();
return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
}
/**
* conjugation
*/
Vector<3> conjugate(const Vector<3> &v) const {
float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3];
return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
);
}
/**
* conjugation with inversed quaternion
*/
Vector<3> conjugate_inversed(const Vector<3> &v) const {
float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3];
return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
);
}
/**
* imaginary part of quaternion
*/
@@ -115,35 +191,6 @@ public:
return Vector<3>(&data[1]);
}
/**
* inverse of quaternion
*/
math::Quaternion inverse() {
Quaternion res;
memcpy(res.data,data,sizeof(res.data));
res.data[1] = -res.data[1];
res.data[2] = -res.data[2];
res.data[3] = -res.data[3];
return res;
}
/**
* rotate vector by quaternion
*/
Vector<3> rotate(const Vector<3> &w) {
Quaternion q_w; // extend vector to quaternion
Quaternion q = {data[0],data[1],data[2],data[3]};
Quaternion q_rotated; // quaternion representation of rotated vector
q_w(0) = 0;
q_w(1) = w.data[0];
q_w(2) = w.data[1];
q_w(3) = w.data[2];
q_rotated = q*q_w*q.inverse();
Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
return res;
}
/**
* set quaternion to rotation defined by euler angles
*/
@@ -164,6 +211,17 @@ public:
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
}
/**
* create Euler angles vector from the quaternion
*/
Vector<3> to_euler() const {
return Vector<3>(
atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
);
}
/**
* set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+13
View File
@@ -94,6 +94,19 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/**
* EKF attitude estimator enabled
*
* If enabled, it uses the older EKF filter.
* However users can enable the new quaternion
* based complimentary filter by setting EKF_ATT_ENABLED = 0.
*
* @min 0
* @max 1
* @group Attitude EKF estimator
*/
PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+479
View File
@@ -0,0 +1,479 @@
/****************************************************************************
*
* Copyright (c) 2015 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 attitude_estimator_q_main.cpp
*
* Attitude estimator (quaternion based)
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <poll.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
using math::Vector;
using math::Matrix;
using math::Quaternion;
class AttitudeEstimatorQ;
namespace attitude_estimator_q {
AttitudeEstimatorQ *instance;
}
class AttitudeEstimatorQ {
public:
/**
* Constructor
*/
AttitudeEstimatorQ();
/**
* Destructor, also kills task.
*/
~AttitudeEstimatorQ();
/**
* Start task.
*
* @return OK on success.
*/
int start();
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
private:
static constexpr float _dt_max = 0.02;
bool _task_should_exit = false; /**< if true, task should exit */
int _control_task = -1; /**< task handle for task */
int _sensors_sub = -1;
int _params_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = -1;
struct {
param_t w_acc;
param_t w_mag;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
bool _acc_comp = false;
float _bias_max = 0.0f;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
vehicle_global_position_s _gpos = {};
Vector<3> _vel_prev;
Vector<3> _pos_acc;
hrt_abstime _vel_prev_t = 0;
bool _inited = false;
perf_counter_t _update_perf;
perf_counter_t _loop_perf;
void update_parameters(bool force);
int update_subscriptions();
void init();
void update(float dt);
};
AttitudeEstimatorQ::AttitudeEstimatorQ() {
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
}
/**
* Destructor, also kills task.
*/
AttitudeEstimatorQ::~AttitudeEstimatorQ() {
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
attitude_estimator_q::instance = nullptr;
}
int AttitudeEstimatorQ::start() {
ASSERT(_control_task == -1);
/* start the task */
_control_task = task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2500,
(main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
attitude_estimator_q::instance->task_main();
}
void AttitudeEstimatorQ::task_main() {
warnx("started");
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
update_parameters(true);
hrt_abstime last_time = 0;
struct pollfd fds[1];
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
continue;
}
update_parameters(false);
// Update sensors
sensor_combined_s sensors;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
_gyro.set(sensors.gyro_rad_s);
_accel.set(sensors.accelerometer_m_s2);
_mag.set(sensors.magnetometer_ga);
}
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
if (gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
/* set magnetic declination automatically */
_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
}
}
if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) {
/* position data is actual */
if (gpos_updated) {
Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
/* velocity updated */
if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = _gpos.timestamp;
_vel_prev = vel;
}
} else {
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
// Time from previous iteration
uint64_t now = hrt_absolute_time();
float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f;
last_time = now;
if (dt > _dt_max) {
dt = _dt_max;
}
update(dt);
Vector<3> euler = _q.to_euler();
struct vehicle_attitude_s att = {};
att.timestamp = sensors.timestamp;
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
}
/* copy offsets */
memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
Matrix<3, 3> R = _q.to_dcm();
/* copy rotation matrix */
memcpy(&att.R[0], R.data, sizeof(att.R));
att.R_valid = true;
if (_att_pub < 0) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
}
}
void AttitudeEstimatorQ::update_parameters(bool force) {
bool updated = force;
if (!updated) {
orb_check(_params_sub, &updated);
}
if (updated) {
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
_mag_decl = math::radians(mag_decl_deg);
int32_t mag_decl_auto_int;
param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
_mag_decl_auto = mag_decl_auto_int != 0;
int32_t acc_comp_int;
param_get(_params_handles.acc_comp, &acc_comp_int);
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
}
}
void AttitudeEstimatorQ::init() {
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
Vector<3> k = -_accel;
k.normalize();
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
Vector<3> i = (_mag - k * (_mag * k));
i.normalize();
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
Vector<3> j = k % i;
// Fill rotation matrix
Matrix<3, 3> R;
R.set_row(0, i);
R.set_row(1, j);
R.set_row(2, k);
// Convert to quaternion
_q.from_dcm(R);
}
void AttitudeEstimatorQ::update(float dt) {
if (!_inited) {
init();
_inited = true;
}
// Angular rate of correction
Vector<3> corr;
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector<3> k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
_rates = _gyro + _gyro_bias;
// Feed forward gyro
corr += _rates;
// Apply correction to state
_q += _q.derivative(corr) * dt;
// Normalize quaternion
_q.normalize(); // TODO! NaN protection???
}
int attitude_estimator_q_main(int argc, char *argv[]) {
if (argc < 1) {
errx(1, "usage: attitude_estimator_q {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (attitude_estimator_q::instance != nullptr) {
errx(1, "already running");
}
attitude_estimator_q::instance = new AttitudeEstimatorQ;
if (attitude_estimator_q::instance == nullptr) {
errx(1, "alloc failed");
}
if (OK != attitude_estimator_q::instance->start()) {
delete attitude_estimator_q::instance;
attitude_estimator_q::instance = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (attitude_estimator_q::instance == nullptr) {
errx(1, "not running");
}
delete attitude_estimator_q::instance;
attitude_estimator_q::instance = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (attitude_estimator_q::instance) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+50
View File
@@ -0,0 +1,50 @@
/****************************************************************************
*
* Copyright (c) 2015 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 attitude_estimator_q_params.c
*
* Parameters for attitude estimator (quaternion based)
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <systemlib/param/param.h>
PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); ///< magnetic declination, in degrees
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); ///< automatic GPS based magnetic declination
PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); ///< acceleration compensation
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s
src/modules/attitude_estimator_q/module.mk
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
#
# Attitude estimator (quaternion based)
#
MODULE_COMMAND = attitude_estimator_q
SRCS = attitude_estimator_q_main.cpp \
attitude_estimator_q_params.c
MODULE_STACKSIZE = 1200
src/modules/commander/accelerometer_calibration.cpp
+32
View File
@@ -407,6 +407,31 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
*/
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
/* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
float board_offset[3];
param_get(board_offset_x, &board_offset[0]);
param_get(board_offset_y, &board_offset[1]);
param_get(board_offset_z, &board_offset[2]);
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0],
M_DEG_TO_RAD_F * board_offset[1],
M_DEG_TO_RAD_F * board_offset[2]);
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
/* combine board rotation with offset rotation */
board_rotation = board_rotation_offset * board_rotation;
struct pollfd fds[max_accel_sens];
for (unsigned i = 0; i < max_accel_sens; i++) {
@@ -453,6 +478,13 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
}
}
// rotate sensor measurements from body frame into sensor frame using board rotation matrix
for (unsigned i = 0; i < max_accel_sens; i++) {
math::Vector<3> accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec;
memcpy(&accel_sum[i][0], &accel_sum_vec.data[0], sizeof(accel_sum[i]));
}
for (unsigned s = 0; s < max_accel_sens; s++) {
for (unsigned i = 0; i < 3; i++) {
accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
src/modules/commander/calibration_routines.cpp
+7 -7
View File
@@ -43,12 +43,12 @@
#include <float.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <mavlink/mavlink_log.h>
#include <geo/geo.h>
#include <string.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/sensor_combined.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
@@ -236,7 +236,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
{
const unsigned ndim = 3;
struct accel_report sensor;
struct sensor_combined_s sensor;
float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel
float ema_len = 0.5f; // EMA time constant in seconds
@@ -264,7 +264,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor);
orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
@@ -275,13 +275,13 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
float di = 0.0f;
switch (i) {
case 0:
di = sensor.x;
di = sensor.accelerometer_m_s2[0];
break;
case 1:
di = sensor.y;
di = sensor.accelerometer_m_s2[1];
break;
case 2:
di = sensor.z;
di = sensor.accelerometer_m_s2[2];
break;
}
@@ -410,7 +410,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
src/modules/commander/commander.cpp
+1 -1
View File
@@ -1160,7 +1160,7 @@ int commander_thread_main(int argc, char *argv[])
/* initialize low priority thread */
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
pthread_attr_setstacksize(&commander_low_prio_attr, 2600);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
src/modules/commander/commander_params.c
+14 -7
View File
@@ -111,7 +111,8 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
/** Datalink loss time threshold
/**
* Datalink loss time threshold
*
* After this amount of seconds without datalink the data link lost mode triggers
*
@@ -122,7 +123,8 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
/** Datalink regain time threshold
/**
* Datalink regain time threshold
*
* After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
* flag is set back to false
@@ -134,7 +136,8 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
*/
PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
/** Engine Failure Throttle Threshold
/**
* Engine Failure Throttle Threshold
*
* Engine failure triggers only above this throttle value
*
@@ -144,7 +147,8 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
*/
PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
/** Engine Failure Current/Throttle Threshold
/**
* Engine Failure Current/Throttle Threshold
*
* Engine failure triggers only below this current/throttle value
*
@@ -154,7 +158,8 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
*/
PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
/** Engine Failure Time Threshold
/**
* Engine Failure Time Threshold
*
* Engine failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time
@@ -166,7 +171,8 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
/** RC loss time threshold
/**
* RC loss time threshold
*
* After this amount of seconds without RC connection the rc lost flag is set to true
*
@@ -177,7 +183,8 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
*/
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
/** Autosaving of params
/**
* Autosaving of params
*
* If not equal to zero the commander will automatically save parameters to persistent storage once changed.
* Default is on, as the interoperability with currently deployed GCS solutions depends on parameters
src/modules/mavlink/mavlink.c
+20
View File
@@ -50,18 +50,25 @@
/**
* MAVLink system ID
* @group MAVLink
* @min 1
* @max 250
*/
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
/**
* MAVLink component ID
* @group MAVLink
* @min 1
* @max 50
*/
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
/**
* MAVLink type
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
/**
* Use/Accept HIL GPS message (even if not in HIL mode)
*
@@ -70,6 +77,7 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
/**
* Forward external setpoint messages
*
@@ -80,6 +88,18 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
*/
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
/**
* Test parameter
*
* This parameter is not actively used by the system. Its purpose is to allow
* testing the parameter interface on the communication level.
*
* @group MAVLink
* @min -1000
* @max 1000
*/
PARAM_DEFINE_INT32(MAV_TEST_PAR, 1);
mavlink_system_t mavlink_system = {
100,
50
src/modules/mavlink/mavlink_ftp.cpp
+271 -215
View File
@@ -42,116 +42,130 @@
#include <errno.h>
#include "mavlink_ftp.h"
#include "mavlink_main.h"
#include "mavlink_tests/mavlink_ftp_test.h"
// Uncomment the line below to get better debug output. Never commit with this left on.
//#define MAVLINK_FTP_DEBUG
MavlinkFTP *
MavlinkFTP::get_server(void)
int buf_size_1 = 0;
int buf_size_2 = 0;
MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
MavlinkStream(mavlink),
_session_info{},
_utRcvMsgFunc{},
_worker_data{}
{
static MavlinkFTP server;
return &server;
// initialize session
_session_info.fd = -1;
}
MavlinkFTP::MavlinkFTP() :
_request_bufs{},
_request_queue{},
_request_queue_sem{},
_utRcvMsgFunc{},
_ftp_test{}
MavlinkFTP::~MavlinkFTP()
{
// initialise the request freelist
dq_init(&_request_queue);
sem_init(&_request_queue_sem, 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++) {
_return_request(&_request_bufs[i]);
const char*
MavlinkFTP::get_name(void) const
{
return "MAVLINK_FTP";
}
uint8_t
MavlinkFTP::get_id(void)
{
return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL;
}
unsigned
MavlinkFTP::get_size(void)
{
if (_session_info.stream_download) {
return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
MavlinkStream*
MavlinkFTP::new_instance(Mavlink *mavlink)
{
return new MavlinkFTP(mavlink);
}
#ifdef MAVLINK_FTP_UNIT_TEST
void
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data)
{
_utRcvMsgFunc = rcvMsgFunc;
_ftp_test = ftp_test;
_worker_data = worker_data;
}
#endif
void
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
uint8_t
MavlinkFTP::_getServerSystemId(void)
{
// get a free request
struct Request* req = _get_request();
// if we couldn't get a request slot, just drop it
if (req == nullptr) {
warnx("Dropping FTP request: queue full\n");
return;
}
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
#ifdef MAVLINK_FTP_UNIT_TEST
if (!_utRcvMsgFunc) {
warnx("Incorrectly written unit test\n");
// We use fake ids when unit testing
return MavlinkFtpTest::serverSystemId;
#else
// Not unit testing, use the real thing
return _mavlink->get_system_id();
#endif
}
uint8_t
MavlinkFTP::_getServerComponentId(void)
{
#ifdef MAVLINK_FTP_UNIT_TEST
// We use fake ids when unit testing
return MavlinkFtpTest::serverComponentId;
#else
// Not unit testing, use the real thing
return _mavlink->get_component_id();
#endif
}
uint8_t
MavlinkFTP::_getServerChannel(void)
{
#ifdef MAVLINK_FTP_UNIT_TEST
// We use fake ids when unit testing
return MavlinkFtpTest::serverChannel;
#else
// Not unit testing, use the real thing
return _mavlink->get_channel();
#endif
}
void
MavlinkFTP::handle_message(const mavlink_message_t *msg)
{
//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
mavlink_file_transfer_protocol_t ftp_request;
mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system);
#endif
if (ftp_request.target_system == _getServerSystemId()) {
_process_request(&ftp_request, msg->sysid);
return;
}
// We use fake ids when unit testing
req->serverSystemId = MavlinkFtpTest::serverSystemId;
req->serverComponentId = MavlinkFtpTest::serverComponentId;
req->serverChannel = MavlinkFtpTest::serverChannel;
#else
// Not unit testing, use the real thing
req->serverSystemId = mavlink->get_system_id();
req->serverComponentId = mavlink->get_component_id();
req->serverChannel = mavlink->get_channel();
#endif
// This is the system id we want to target when sending
req->targetSystemId = msg->sysid;
if (req->message.target_system == req->serverSystemId) {
req->mavlink = mavlink;
#ifdef MAVLINK_FTP_UNIT_TEST
// We are running in Unit Test mode. Don't queue, just call _worket directly.
_process_request(req);
#else
// We are running in normal mode. Queue the request to the worker
work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
#endif
return;
}
}
_return_request(req);
}
/// @brief Queued static work queue routine to handle mavlink messages
void
MavlinkFTP::_worker_trampoline(void *arg)
{
Request* req = reinterpret_cast<Request *>(arg);
MavlinkFTP* server = MavlinkFTP::get_server();
// call the server worker with the work item
server->_process_request(req);
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(Request *req)
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
ErrorCode errorCode = kErrNone;
@@ -162,7 +176,7 @@ MavlinkFTP::_process_request(Request *req)
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset);
#endif
switch (payload->opcode) {
@@ -197,6 +211,11 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRead(payload);
break;
case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
stream_send = true;
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
@@ -221,7 +240,6 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
@@ -232,16 +250,14 @@ MavlinkFTP::_process_request(Request *req)
}
out:
payload->seq_number++;
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: ack\n");
#endif
} else {
int r_errno = errno;
warnx("FTP: nak %u", errorCode);
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
@@ -252,60 +268,33 @@ out:
}
}
// respond to the request
_reply(req);
_return_request(req);
// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nak.
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
_reply(ftp_req);
}
}
/// @brief Sends the specified FTP reponse message out through mavlink
/// @brief Sends the specified FTP response message out through mavlink
void
MavlinkFTP::_reply(Request *req)
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
payload->seqNumber = payload->seqNumber + 1;
mavlink_message_t msg;
msg.checksum = 0;
#ifndef MAVLINK_FTP_UNIT_TEST
uint16_t len =
#ifdef MAVLINK_FTP_DEBUG
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#endif
mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id
req->serverComponentId, // Sender component id
req->serverChannel, // Channel to send on
&msg, // Message to pack payload into
0, // Target network
req->targetSystemId, // Target system id
0, // Target component id
(const uint8_t*)payload); // Payload to pack into message
bool success = true;
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(&msg, _ftp_test);
_utRcvMsgFunc(ftp_req, _worker_data);
#else
Mavlink *mavlink = req->mavlink;
mavlink->lockMessageBufferMutex();
success = mavlink->message_buffer_write(&msg, len);
mavlink->unlockMessageBufferMutex();
_mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req);
#endif
if (!success) {
warnx("FTP TX ERR");
}
#ifdef MAVLINK_FTP_DEBUG
else {
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
req->serverSystemId,
req->serverComponentId,
req->serverChannel,
msg.checksum);
}
#endif
}
/// @brief Responds to a List command
@@ -417,13 +406,16 @@ MavlinkFTP::_workList(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
{
int session_index = _find_unused_session();
if (session_index < 0) {
if (_session_info.fd >= 0) {
warnx("FTP: Open failed - out of sessions\n");
return kErrNoSessionsAvailable;
}
char *filename = _data_as_cstring(payload);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: open '%s'", filename);
#endif
uint32_t fileSize = 0;
struct stat st;
@@ -440,9 +432,11 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
if (fd < 0) {
return kErrFailErrno;
}
_session_fds[session_index] = fd;
_session_info.fd = fd;
_session_info.file_size = fileSize;
_session_info.stream_download = false;
payload->session = session_index;
payload->session = 0;
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
@@ -453,23 +447,25 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(PayloadHeader* payload)
{
int session_index = payload->session;
if (!_valid_session(session_index)) {
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
}
// Seek to the specified position
#ifdef MAVLINK_FTP_DEBUG
warnx("seek %d", payload->offset);
warnx("FTP: read offset:%d", payload->offset);
#endif
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
warnx("seek fail");
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (payload->offset >= _session_info.file_size) {
warnx("request past EOF");
return kErrEOF;
}
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
warnx("seek fail");
return kErrFailErrno;
}
int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
warnx("read fail %d", bytes_read);
@@ -481,27 +477,41 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
return kErrNone;
}
/// @brief Responds to a Stream command
MavlinkFTP::ErrorCode
MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
{
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: burst offset:%d", payload->offset);
#endif
// Setup for streaming sends
_session_info.stream_download = true;
_session_info.stream_offset = payload->offset;
_session_info.stream_seq_number = payload->seq_number + 1;
_session_info.stream_target_system_id = target_system_id;
return kErrNone;
}
/// @brief Responds to a Write command
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload)
{
int session_index = payload->session;
if (!_valid_session(session_index)) {
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
}
// Seek to the specified position
#ifdef MAVLINK_FTP_DEBUG
warnx("seek %d", payload->offset);
#endif
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
warnx("seek fail");
return kErrFailErrno;
}
int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size);
if (bytes_written < 0) {
// Negative return indicates error other than eof
warnx("write fail %d", bytes_written);
@@ -608,12 +618,13 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload)
{
if (!_valid_session(payload->session)) {
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
}
::close(_session_fds[payload->session]);
_session_fds[payload->session] = -1;
::close(_session_info.fd);
_session_info.fd = -1;
_session_info.stream_download = false;
payload->size = 0;
@@ -624,11 +635,10 @@ MavlinkFTP::_workTerminate(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(PayloadHeader* payload)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
if (_session_info.fd != -1) {
::close(_session_info.fd);
_session_info.fd = -1;
_session_info.stream_download = false;
}
payload->size = 0;
@@ -726,29 +736,6 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
return kErrNone;
}
/// @brief Returns true if the specified session is a valid open session
bool
MavlinkFTP::_valid_session(unsigned index)
{
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
return false;
}
return true;
}
/// @brief Returns an unused session index
int
MavlinkFTP::_find_unused_session(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
return i;
}
}
return -1;
}
/// @brief Guarantees that the payload data is null terminated.
/// @return Returns a pointer to the payload data as a char *
char *
@@ -765,40 +752,6 @@ MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
return (char *)&(payload->data[0]);
}
/// @brief Returns a unused Request entry. NULL if none available.
MavlinkFTP::Request *
MavlinkFTP::_get_request(void)
{
_lock_request_queue();
Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
_unlock_request_queue();
return req;
}
/// @brief Locks a semaphore to provide exclusive access to the request queue
void
MavlinkFTP::_lock_request_queue(void)
{
do {}
while (sem_wait(&_request_queue_sem) != 0);
}
/// @brief Unlocks the semaphore providing exclusive access to the request queue
void
MavlinkFTP::_unlock_request_queue(void)
{
sem_post(&_request_queue_sem);
}
/// @brief Returns a no longer needed request to the queue
void
MavlinkFTP::_return_request(Request *req)
{
_lock_request_queue();
dq_addlast(&req->work.dq, &_request_queue);
_unlock_request_queue();
}
/// @brief Copy file (with limited space)
int
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
@@ -851,3 +804,106 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
errno = op_errno;
return (length > 0)? -1 : 0;
}
void MavlinkFTP::send(const hrt_abstime t)
{
// Anything to stream?
if (!_session_info.stream_download) {
return;
}
#ifndef MAVLINK_FTP_UNIT_TEST
// Skip send if not enough room
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
#ifdef MAVLINK_FTP_DEBUG
warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf());
#endif
if (max_bytes_to_send < get_size()) {
return;
}
#endif
// Send stream packets until buffer is full
bool more_data;
do {
more_data = false;
ErrorCode error_code = kErrNone;
mavlink_file_transfer_protocol_t ftp_msg;
PayloadHeader* payload = reinterpret_cast<PayloadHeader *>(&ftp_msg.payload[0]);
payload->seq_number = _session_info.stream_seq_number;
payload->session = 0;
payload->opcode = kRspAck;
payload->req_opcode = kCmdBurstReadFile;
payload->offset = _session_info.stream_offset;
_session_info.stream_seq_number++;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream send: offset %d", _session_info.stream_offset);
#endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (_session_info.stream_offset >= _session_info.file_size) {
error_code = kErrEOF;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: sending Nak EOF");
#endif
}
if (error_code == kErrNone) {
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: seek fail");
#endif
}
}
if (error_code == kErrNone) {
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: read fail");
#endif
} else {
payload->size = bytes_read;
_session_info.stream_offset += bytes_read;
}
}
if (error_code != kErrNone) {
payload->opcode = kRspNak;
payload->size = 1;
uint8_t* pData = &payload->data[0];
*pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error
if (error_code == kErrFailErrno) {
int r_errno = errno;
payload->size = 2;
payload->data[1] = r_errno;
}
_session_info.stream_download = false;
} else {
#ifndef MAVLINK_FTP_UNIT_TEST
if (max_bytes_to_send < (get_size()*2)) {
more_data = false;
payload->burst_complete = true;
_session_info.stream_download = false;
} else {
#endif
more_data = true;
payload->burst_complete = false;
#ifndef MAVLINK_FTP_UNIT_TEST
max_bytes_to_send -= get_size();
}
#endif
}
ftp_msg.target_system = _session_info.stream_target_system_id;
_reply(&ftp_msg);
} while (more_data);
}
src/modules/mavlink/mavlink_ftp.h
+51 -50
View File
@@ -39,45 +39,44 @@
#include <dirent.h>
#include <queue.h>
#include <nuttx/wqueue.h>
#include <systemlib/err.h>
#include "mavlink_messages.h"
#include "mavlink_main.h"
#include "mavlink_stream.h"
#include "mavlink_bridge_header.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
/// MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
class MavlinkFTP : public MavlinkStream
{
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();
MavlinkFTP(Mavlink *mavlink);
~MavlinkFTP();
static MavlinkStream *new_instance(Mavlink *mavlink);
/// @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);
/// Handle possible FTP message
void handle_message(const mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
/// @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);
/// @param worker_data Data to pass to worker
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data);
/// @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
uint16_t seq_number; ///< 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
uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming.
uint8_t padding; ///< 32 bit aligment padding
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[]; ///< command data, varies by Opcode
};
@@ -100,6 +99,7 @@ public:
kCmdTruncateFile, ///< Truncate file at <path> to <offset> length
kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kCmdBurstReadFile, ///< Burst download session file
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
@@ -118,35 +118,22 @@ public:
kErrUnknownCommand ///< Unknown command opcode
};
// MavlinkStream overrides
virtual const char *get_name(void) const;
virtual uint8_t get_id(void);
virtual unsigned get_size(void);
private:
/// @brief Unit of work which is queued to work_queue
struct Request
{
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);
void _process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t* ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader* payload);
@@ -156,14 +143,13 @@ private:
ErrorCode _workTruncateFile(PayloadHeader *payload);
ErrorCode _workRename(PayloadHeader *payload);
ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
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);
uint8_t _getServerSystemId(void);
uint8_t _getServerComponentId(void);
uint8_t _getServerChannel(void);
// Overrides from MavlinkStream
virtual void send(const hrt_abstime t);
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
@@ -172,9 +158,24 @@ private:
/// @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
struct SessionInfo {
int fd;
uint32_t file_size;
bool stream_download;
uint32_t stream_offset;
uint16_t stream_seq_number;
uint8_t stream_target_system_id;
};
struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc;
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
void *_worker_data; ///< Additional parameter to _utRcvMsgFunc;
/* do not allow copying this class */
MavlinkFTP(const MavlinkFTP&);
MavlinkFTP operator=(const MavlinkFTP&);
// Mavlink test needs to be able to call send
friend class MavlinkFtpTest;
};
src/modules/mavlink/mavlink_main.cpp
+12
View File
@@ -131,6 +131,7 @@ Mavlink::Mavlink() :
_streams(nullptr),
_mission_manager(nullptr),
_parameters_manager(nullptr),
_mavlink_ftp(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
@@ -486,6 +487,9 @@ void Mavlink::mavlink_update_system(void)
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
/* test param - needs to be referenced, but is unused */
(void)param_find("MAV_TEST_PAR");
}
/* update system and component id */
@@ -868,6 +872,9 @@ Mavlink::handle_message(const mavlink_message_t *msg)
/* handle packet with parameter component */
_parameters_manager->handle_message(msg);
/* handle packet with ftp component */
_mavlink_ftp->handle_message(msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
@@ -1364,6 +1371,11 @@ Mavlink::task_main(int argc, char *argv[])
_parameters_manager->set_interval(interval_from_rate(120.0f));
LL_APPEND(_streams, _parameters_manager);
/* MAVLINK_FTP stream */
_mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this);
_mavlink_ftp->set_interval(interval_from_rate(120.0f));
LL_APPEND(_streams, _mavlink_ftp);
/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
* remote requests rate. Rate specified here controls how much bandwidth we will reserve for
* mission messages. */
src/modules/mavlink/mavlink_main.h
+4 -2
View File
@@ -59,6 +59,7 @@
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
#include "mavlink_ftp.h"
class Mavlink
{
@@ -296,8 +297,9 @@ private:
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
MavlinkFTP *_mavlink_ftp;
MAVLINK_MODE _mode;
src/modules/mavlink/mavlink_receiver.cpp
-6
View File
@@ -134,8 +134,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_time_offset(0)
{
// make sure the FTP server is started
(void)MavlinkFTP::get_server();
}
MavlinkReceiver::~MavlinkReceiver()
@@ -202,10 +200,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
handle_message_system_time(msg);
break;
src/modules/mavlink/mavlink_stream.h
-2
View File
@@ -73,8 +73,6 @@ public:
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
static MavlinkStream *new_instance(const Mavlink *mavlink);
static const char *get_name_static();
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+242 -96
View File
@@ -43,19 +43,19 @@
#include "../mavlink_ftp.h"
/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets
const MavlinkFtpTest::DownloadTestCase MavlinkFtpTest::_rgDownloadTestCases[] = {
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1, true, false }, // Read takes less than single packet
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader), true, true }, // Read completely fills single packet
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1, false, false }, // Read take two packets
};
const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
MavlinkFtpTest::MavlinkFtpTest() :
_ftp_server{},
_reply_msg{},
_lastOutgoingSeqNumber{}
_ftp_server(nullptr),
_expected_seq_number(0),
_reply_msg{}
{
}
@@ -67,8 +67,9 @@ MavlinkFtpTest::~MavlinkFtpTest()
/// @brief Called before every test to initialize the FTP Server.
void MavlinkFtpTest::_init(void)
{
_ftp_server = new MavlinkFTP;;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
_expected_seq_number = 0;
_ftp_server = new MavlinkFTP(NULL);
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
_cleanup_microsd();
}
@@ -85,15 +86,13 @@ void MavlinkFtpTest::_cleanup(void)
bool MavlinkFtpTest::_ack_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = MavlinkFTP::kCmdNone;
bool success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -109,15 +108,13 @@ bool MavlinkFtpTest::_ack_test(void)
bool MavlinkFtpTest::_bad_opcode_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = 0xFF; // bogus opcode
bool success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -135,8 +132,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
{
mavlink_message_t msg;
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = MavlinkFTP::kCmdListDirectory;
@@ -145,9 +141,9 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
// Set the data size to be one larger than is legal
((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
_ftp_server->handle_message(&msg);
if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
if (!_decode_message(&_reply_msg, &reply)) {
return false;
}
@@ -161,8 +157,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
bool MavlinkFtpTest::_list_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
char response2[] = "Ddev|Detc|Dfs|Dobj";
@@ -188,7 +183,6 @@ bool MavlinkFtpTest::_list_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->dir)+1, // size in bytes of data
(uint8_t*)test->dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -202,16 +196,20 @@ bool MavlinkFtpTest::_list_test(void)
// to a hardcoded return result string.
// Convert null terminators to seperator char so we can use strok to parse returned data
char list_entry[256];
for (uint8_t j=0; j<reply->size-1; j++) {
if (reply->data[j] == 0) {
reply->data[j] = '|';
list_entry[j] = '|';
} else {
list_entry[j] = reply->data[j];
}
}
list_entry[reply->size-1] = 0;
// Loop over returned directory entries trying to find then in the response list
char *dir;
int response_count = 0;
dir = strtok((char *)&reply->data[0], "|");
dir = strtok(list_entry, "|");
while (dir != nullptr) {
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
response_count++;
@@ -234,8 +232,7 @@ bool MavlinkFtpTest::_list_test(void)
bool MavlinkFtpTest::_list_eof_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
const char *dir = "/";
payload.opcode = MavlinkFTP::kCmdListDirectory;
@@ -244,7 +241,6 @@ bool MavlinkFtpTest::_list_eof_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(dir)+1, // size in bytes of data
(uint8_t*)dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -261,8 +257,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
bool MavlinkFtpTest::_open_badfile_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
const char *dir = "/foo"; // non-existent file
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
@@ -271,7 +266,6 @@ bool MavlinkFtpTest::_open_badfile_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(dir)+1, // size in bytes of data
(uint8_t*)dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -288,12 +282,11 @@ bool MavlinkFtpTest::_open_badfile_test(void)
bool MavlinkFtpTest::_open_terminate_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
const DownloadTestCase *test = &_rgDownloadTestCases[i];
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
@@ -301,7 +294,6 @@ bool MavlinkFtpTest::_open_terminate_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -321,7 +313,6 @@ bool MavlinkFtpTest::_open_terminate_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -338,9 +329,8 @@ bool MavlinkFtpTest::_open_terminate_test(void)
bool MavlinkFtpTest::_terminate_badsession_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
const MavlinkFTP::PayloadHeader *reply;
const char *file = _rgDownloadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
@@ -348,7 +338,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(file)+1, // size in bytes of data
(uint8_t*)file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -363,7 +352,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -380,12 +368,11 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
bool MavlinkFtpTest::_read_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
const DownloadTestCase *test = &_rgDownloadTestCases[i];
// Read in the file so we can compare it to what we get back
ut_compare("stat failed", stat(test->file, &st), 0);
@@ -406,7 +393,6 @@ bool MavlinkFtpTest::_read_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -421,7 +407,6 @@ bool MavlinkFtpTest::_read_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -429,10 +414,40 @@ bool MavlinkFtpTest::_read_test(void)
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, 0);
uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0);
if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
payload.offset += expected_bytes;
if (test->singlePacketRead) {
// Try going past EOF
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
} else {
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, payload.offset);
expected_bytes = (uint32_t)st.st_size - full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0);
}
payload.opcode = MavlinkFTP::kCmdTerminateSession;
@@ -442,7 +457,91 @@ bool MavlinkFtpTest::_read_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Incorrect payload size", reply->size, 0);
}
return true;
}
/// @brief Tests for correct reponse to a Read command on an open session.
bool MavlinkFtpTest::_burst_test(void)
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
BurstInfo burst_info;
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const DownloadTestCase *test = &_rgDownloadTestCases[i];
// Read in the file so we can compare it to what we get back
ut_compare("stat failed", stat(test->file, &st), 0);
uint8_t *bytes = new uint8_t[st.st_size];
ut_assert("new failed", bytes != nullptr);
int fd = ::open(test->file, O_RDONLY);
ut_assert("open failed", fd != -1);
int bytes_read = ::read(fd, bytes, st.st_size);
ut_compare("read failed", bytes_read, st.st_size);
::close(fd);
// Test case data files are created for specific boundary conditions
ut_compare("Test case data files are out of date", test->length, st.st_size);
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
// Setup for burst response handler
burst_info.burst_state = burst_state_first_ack;
burst_info.single_packet_file = test->singlePacketRead;
burst_info.file_size = st.st_size;
burst_info.file_bytes = bytes;
burst_info.ftp_test_class = this;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_burst, &burst_info);
// Send the burst command, message response will be handled by _receive_message_handler_stream
payload.opcode = MavlinkFTP::kCmdBurstReadFile;
payload.session = reply->session;
payload.offset = 0;
mavlink_message_t msg;
_setup_ftp_msg(&payload, 0, nullptr, &msg);
_ftp_server->handle_message(&msg);
// First packet is sent using stream mechanism, so we need to force it out ourselves
hrt_abstime t = 0;
_ftp_server->send(t);
ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete);
// Put back generic message handler
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
// Terminate session
payload.opcode = MavlinkFTP::kCmdTerminateSession;
payload.session = reply->session;
payload.size = 0;
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -459,18 +558,16 @@ bool MavlinkFtpTest::_read_test(void)
bool MavlinkFtpTest::_read_badsession_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
const MavlinkFTP::PayloadHeader *reply;
const char *file = _rgDownloadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(file)+1, // size in bytes of data
(uint8_t*)file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
@@ -484,7 +581,6 @@ bool MavlinkFtpTest::_read_badsession_test(void)
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -500,8 +596,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
bool MavlinkFtpTest::_removedirectory_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
int fd;
struct _testCase {
@@ -534,7 +629,6 @@ bool MavlinkFtpTest::_removedirectory_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->dir)+1, // size in bytes of data
(uint8_t*)test->dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -556,8 +650,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
bool MavlinkFtpTest::_createdirectory_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
struct _testCase {
const char *dir;
@@ -579,7 +672,6 @@ bool MavlinkFtpTest::_createdirectory_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->dir)+1, // size in bytes of data
(uint8_t*)test->dir, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -601,8 +693,7 @@ bool MavlinkFtpTest::_createdirectory_test(void)
bool MavlinkFtpTest::_removefile_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
const MavlinkFTP::PayloadHeader *reply;
int fd;
struct _testCase {
@@ -611,7 +702,7 @@ bool MavlinkFtpTest::_removefile_test(void)
};
static const struct _testCase rgTestCases[] = {
{ "/bogus", false },
{ _rgReadTestCases[0].file, false },
{ _rgDownloadTestCases[0].file, false },
{ _unittest_microsd_dir, false },
{ _unittest_microsd_file, true },
{ _unittest_microsd_file, false },
@@ -630,7 +721,6 @@ bool MavlinkFtpTest::_removefile_test(void)
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&ftp_msg, // Response from server
&reply); // Payload inside FTP message response
if (!success) {
return false;
@@ -649,62 +739,119 @@ bool MavlinkFtpTest::_removefile_test(void)
return true;
}
/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
/// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when
/// it needs to send a message out on Mavlink.
void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
{
ftp_test->_receive_message(msg);
((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req);
}
/// @brief Non-Static version of receive_message
void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req)
{
// Move the message into our own member variable
memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t));
}
/// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when
/// it needs to send a message out on Mavlink.
void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
{
BurstInfo* burst_info = (BurstInfo*)worker_data;
burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info);
}
bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info)
{
hrt_abstime t = 0;
const MavlinkFTP::PayloadHeader* reply;
uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
uint32_t expected_bytes;
_decode_message(ftp_msg, &reply);
switch (burst_info->burst_state) {
case burst_state_first_ack:
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, 0);
expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("burst_complete incorrect", reply->burst_complete, 0);
ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0);
// Setup for next expected message
burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack;
ut_assert("Remaining stream packets missing", _ftp_server->get_size());
_ftp_server->send(t);
break;
case burst_state_last_ack:
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, full_packet_bytes);
expected_bytes = burst_info->file_size - full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("burst_complete incorrect", reply->burst_complete, 0);
ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0);
// Setup for next expected message
burst_info->burst_state = burst_state_nak_eof;
ut_assert("Remaining stream packets missing", _ftp_server->get_size());
_ftp_server->send(t);
break;
case burst_state_nak_eof:
// Signal complete
burst_info->burst_state = burst_state_complete;
ut_compare("All packets should have been seent", _ftp_server->get_size(), 0);
break;
}
return true;
}
/// @brief Decode and validate the incoming message
bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode
mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message
MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, ///< Incoming FTP message
const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
{
mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
//warnx("_decode_message");
// Make sure the targets are correct
ut_compare("Target network non-zero", ftp_msg->target_network, 0);
ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
*payload = reinterpret_cast<const MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
// Make sure we have a good sequence number
ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
// Bump sequence number for next outgoing message
_lastOutgoingSeqNumber++;
ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number);
_expected_seq_number++;
return true;
}
/// @brief Initializes an FTP message into a mavlink message
void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
uint8_t size, ///< size in bytes of data
const uint8_t *data, ///< Data to start into FTP message payload
mavlink_message_t *msg) ///< Returned mavlink message
void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
uint8_t size, ///< size in bytes of data
const uint8_t *data, ///< Data to start into FTP message payload
mavlink_message_t *msg) ///< Returned mavlink message
{
uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
payload->seqNumber = _lastOutgoingSeqNumber;
payload->seq_number = _expected_seq_number++;
payload->size = size;
if (size != 0) {
memcpy(payload->data, data, size);
}
payload->padding[0] = 0;
payload->padding[1] = 0;
payload->burst_complete = 0;
payload->padding = 0;
msg->checksum = 0;
mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id
@@ -720,14 +867,13 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, /
bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
uint8_t size, ///< size in bytes of data
const uint8_t *data, ///< Data to start into FTP message payload
mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server
MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
{
mavlink_message_t msg;
_setup_ftp_msg(payload_header, size, data, &msg);
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
_ftp_server->handle_message(&msg);
return _decode_message(&_reply_msg, payload_reply);
}
/// @brief Cleans up an files created on microsd during testing
@@ -743,14 +889,14 @@ bool MavlinkFtpTest::run_tests(void)
ut_run_test(_ack_test);
ut_run_test(_bad_opcode_test);
ut_run_test(_bad_datasize_test);
printf("WARNING! list test commented out, but needs proper resolution!\n");
//ut_run_test(_list_test);
ut_run_test(_list_test);
ut_run_test(_list_eof_test);
ut_run_test(_open_badfile_test);
ut_run_test(_open_terminate_test);
ut_run_test(_terminate_badsession_test);
ut_run_test(_read_test);
ut_run_test(_read_badsession_test);
ut_run_test(_burst_test);
ut_run_test(_removedirectory_test);
ut_run_test(_createdirectory_test);
ut_run_test(_removefile_test);
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+39 -14
View File
@@ -48,7 +48,18 @@ public:
virtual bool run_tests(void);
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
/// Worker data for stream handler
struct BurstInfo {
MavlinkFtpTest* ftp_test_class;
int burst_state;
bool single_packet_file;
uint32_t file_size;
uint8_t* file_bytes;
};
static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
static const uint8_t serverSystemId = 50; ///< System ID for server
static const uint8_t serverComponentId = 1; ///< Component ID for server
@@ -75,31 +86,45 @@ private:
bool _terminate_badsession_test(void);
bool _read_test(void);
bool _read_badsession_test(void);
bool _burst_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);
void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req);
void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const 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);
const MavlinkFTP::PayloadHeader **payload_reply);
void _cleanup_microsd(void);
MavlinkFTP *_ftp_server;
mavlink_message_t _reply_msg;
uint16_t _lastOutgoingSeqNumber;
struct ReadTestCase {
/// A single download test case
struct DownloadTestCase {
const char *file;
const uint16_t length;
bool singlePacketRead;
bool exactlyFillPacket;
};
static const ReadTestCase _rgReadTestCases[];
/// The set of test cases for download testing
static const DownloadTestCase _rgDownloadTestCases[];
/// States for stream download handler
enum {
burst_state_first_ack,
burst_state_last_ack,
burst_state_nak_eof,
burst_state_complete
};
bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info);
MavlinkFTP* _ftp_server;
uint16_t _expected_seq_number;
mavlink_file_transfer_protocol_t _reply_msg;
static const char _unittest_microsd_dir[];
static const char _unittest_microsd_file[];
src/modules/mavlink/mavlink_tests/module.mk
+1
View File
@@ -38,6 +38,7 @@
MODULE_COMMAND = mavlink_tests
SRCS = mavlink_tests.cpp \
mavlink_ftp_test.cpp \
../mavlink_stream.cpp \
../mavlink_ftp.cpp \
../mavlink.c
src/modules/position_estimator_inav/position_estimator_inav_params.c
+2 -2
View File
@@ -291,8 +291,8 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
/**
* INAV enabled
*
* If set to 1, use INAV for position estimation
* the system uses the combined attitude / position
* If set to 1, use INAV for position estimation.
* Else the system uses the combined attitude / position
* filter framework.
*
* @min 0
src/modules/px4iofirmware/mixer.cpp
+2 -4
View File
@@ -272,9 +272,8 @@ mixer_tick(void)
if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) {
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
}
/* set S.BUS1 or S.BUS2 outputs */
@@ -286,9 +285,8 @@ mixer_tick(void)
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) {
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
src/modules/px4iofirmware/px4io.h
+1 -2
View File
@@ -51,8 +51,7 @@
/*
* Constants and limits.
*/
#define PX4IO_SERVO_COUNT 16
#define PX4IO_SERVO_HARDWARE_COUNT 8
#define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_CONTROL_GROUPS 4
#define PX4IO_RC_INPUT_CHANNELS 18
src/modules/px4iofirmware/registers.c
+1 -1
View File
@@ -285,7 +285,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_DIRECT_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
if (*values != PWM_IGNORE_THIS_CHANNEL) {
src/modules/px4iofirmware/sbus.c
+2 -2
View File
@@ -163,8 +163,8 @@ sbus1_output(uint16_t *values, uint16_t num_values)
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
// XXX S.BUS2 is not implemented, fall back to S.BUS1
sbus1_output(values, num_values);
char b = 'B';
write(sbus_fd, &b, 1);
}
bool
src/systemcmds/tests/test_mathlib.cpp
+6 -6
View File
@@ -300,7 +300,7 @@ int test_mathlib(int argc, char *argv[])
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.rotate(vector);
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
@@ -315,7 +315,7 @@ int test_mathlib(int argc, char *argv[])
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.rotate(vector);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
@@ -326,7 +326,7 @@ int test_mathlib(int argc, char *argv[])
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.rotate(vector);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
@@ -337,7 +337,7 @@ int test_mathlib(int argc, char *argv[])
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.rotate(vector);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
@@ -348,7 +348,7 @@ int test_mathlib(int argc, char *argv[])
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.rotate(vector);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
@@ -359,4 +359,4 @@ int test_mathlib(int argc, char *argv[])
}
}
return rc;
}
}