diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 2c7c0d68e2..0dd4837644 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 6517e026ab..beb9891d2e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/pass.aux.mix b/ROMFS/px4fmu_common/mixers/pass.aux.mix new file mode 100644 index 0000000000..8e7011f0ed --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/pass.aux.mix @@ -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 diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 0d2413a75f..048836a4eb 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -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*;') diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 07cced4786..b072ab79f8 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -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") diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index cbfda9735f..7884b94cb0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -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 diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index af3ca249e5..ebe7a09c20 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -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) \ diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 1e27309d83..ae75d3a14a 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -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; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5a3104fa5a..0b24ef1526 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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)); diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f6f4fc5ead..2a7b61238b 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -135,6 +135,24 @@ public: } #endif + /** + * set row from vector + */ + void set_row(unsigned int row, const Vector 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 v) { + for (unsigned i = 0; i < M; i++) { + data[i][col] = v.data[i]; + } + } + /** * access by index */ diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index d28966fca6..b7cb068dd4 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -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(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 diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index fe480e12b7..e981c6eb74 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -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); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp new file mode 100644 index 0000000000..a1c56aa1f8 --- /dev/null +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +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, ¶m_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; +} diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c new file mode 100644 index 0000000000..fa15923076 --- /dev/null +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -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 + */ + +#include + +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 diff --git a/src/modules/attitude_estimator_q/module.mk b/src/modules/attitude_estimator_q/module.mk new file mode 100644 index 0000000000..b3688e4a9a --- /dev/null +++ b/src/modules/attitude_estimator_q/module.mk @@ -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 diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f83640d28f..efd88b3d40 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -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]; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 7e8c0fa52e..e854c9aa7e 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -43,12 +43,12 @@ #include #include #include -#include #include #include #include #include +#include #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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5caf36e19a..50846ff4d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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, ¶m); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6663525cc4..5f43fd77cd 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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 diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 796d5cbf28..30c2d2b956 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -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 diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4ba595a87b..10b94b6b26 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -42,116 +42,130 @@ #include #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; imsgid == 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(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(&req->message.payload[0]); + bool stream_send = false; + PayloadHeader *payload = reinterpret_cast(&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(&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(&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; isize = 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; idata[0]); } -/// @brief Returns a unused Request entry. NULL if none available. -MavlinkFTP::Request * -MavlinkFTP::_get_request(void) -{ - _lock_request_queue(); - Request* req = reinterpret_cast(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(&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); +} + diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 9693a92a97..af8740e481 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -39,45 +39,44 @@ #include #include -#include #include -#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 to length kCmdRename, ///< Rename to kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + 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; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 22ff3edf6f..326b0b5ab4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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. */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c285bc4052..90b84061f6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4e332bf1a..4d96f389db 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 5e39bbbdf9..e0a18cce70 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -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; diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 7b5b9228c8..6fe0ed9c8f 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -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; jsize-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; ifile)+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; ifile, &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; ifile, &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(ftp_msg->payload); + *payload = reinterpret_cast(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(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); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index 2696192cc6..14c9369b05 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -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[]; diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index b46d2bd355..e104860937 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = mavlink_tests SRCS = mavlink_tests.cpp \ mavlink_ftp_test.cpp \ + ../mavlink_stream.cpp \ ../mavlink_ftp.cpp \ ../mavlink.c diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index aedb478faa..96b12f9e09 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -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 diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6fa26d4fff..f14599a247 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 8ddf45a12d..a7ac74c33e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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 diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index e7976446cd..a8009da414 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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) { diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 14d8ccca2e..9d28490907 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -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 diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 3a890c30b6..7460f6f559 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -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; -} \ No newline at end of file +}