From 7b05165249fb47bb8ff5d213f0b9e43940def663 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:15:23 +0200 Subject: [PATCH 01/16] Param unit test: Fix CLANG compile warning --- unittests/param_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 44ae4df068..bda49ae86d 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -52,8 +52,8 @@ void _assert_parameter_int_value(param_t param, int32_t expected) { int32_t value; int result = param_get(param, &value); - ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); - ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); + ASSERT_EQ(0, result) << printf("param_get (%lu) did not return parameter\n", param); + ASSERT_EQ(expected, value) << printf("value for param (%lu) doesn't match default value\n", param); } void _set_all_int_parameters_to(int32_t value) From 5982eaaf34f53accbf8040c81e5c0ece3ed6487b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 10:54:09 +0200 Subject: [PATCH 02/16] MC pos control: Enforce minimum throttle in manual attitude control mode only if not landed, else default to idle throttle --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ad1ba56652..54c7608632 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1414,7 +1414,11 @@ MulticopterPositionControl::task_main() /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = math::min(_manual.z, _params.thr_max); - _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + + /* enforce minimum throttle if not landed */ + if (!_vehicle_status.condition_landed) { + _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + } } /* construct attitude setpoint rotation matrix */ From 9a3658836154bea32f0f02b3695fd3deddbcbadb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:15:27 +0200 Subject: [PATCH 03/16] MC land detector: If no position information is available, rely on the armed state exclusively to infer the landed condition. --- src/modules/land_detector/MulticopterLandDetector.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1490232a4f..5391f7769a 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -100,6 +100,14 @@ bool MulticopterLandDetector::update() return true; } + // return status based on armed state if no position lock is available + if (_vehicleGlobalPosition.timestamp == 0 || + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + + // no position lock - not landed if armed + return !_arming.armed; + } + const uint64_t now = hrt_absolute_time(); // check if we are moving vertically From 5549d480fd55817262cb70ebff299a641285c2c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:16:17 +0200 Subject: [PATCH 04/16] MC land detector: Update params and fix docs. Allow more motion during the landed state, but become more picky on throttle. --- .../land_detector/land_detector_params.c | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index f182495ac3..56f971dd35 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -43,29 +43,29 @@ /** * Multicopter max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) * * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 1.00f); /** * Multicopter max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) * * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.50f); /** * Multicopter max rotation * - * Maximum allowed around each axis to trigger a land (degrees per second) + * Maximum allowed around each axis allowed in the landed state (degrees per second) * * @unit deg/s * @@ -76,19 +76,19 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); /** * Multicopter max throttle * - * Maximum actuator output on throttle before triggering a land + * Maximum actuator output on throttle allowed in the landed state * * @min 0.1 * @max 0.5 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); /** * Fixedwing max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) * * @min 0.5 * @max 10 @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) * * @min 5 * @max 20 @@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * - * Maximum airspeed allowed to trigger a land (m/s) + * Maximum airspeed allowed in the landed state (m/s) * * @min 4 * @max 20 From 5bec38b37dbdf87720b98021850141e817de4191 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:17:39 +0200 Subject: [PATCH 05/16] MC land detector: Slightly decrease allowed vertical motion during landed state. This is important so that fast descends do not result in a false positive landed state --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 56f971dd35..77bac2ad74 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -49,7 +49,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f); /** * Multicopter max horizontal velocity From c28a69fba8873b7551f1031e32f480c4f9a522ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:36:59 +0200 Subject: [PATCH 06/16] Mixer test: Ensure its not susceptible to timing jitter of the test harness --- src/systemcmds/tests/test_mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index e9500d2d16..d0eb1eb1ff 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -200,7 +200,7 @@ int test_mixer(int argc, char *argv[]) hrt_abstime starttime = hrt_absolute_time(); unsigned sleepcount = 0; - while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) { /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); From ece87a3fa2afd4e6aa1ab7c85fd15fa42ba05515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:33:52 +0200 Subject: [PATCH 07/16] Mixer test: Fixed compile warnings --- src/systemcmds/tests/test_mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d0eb1eb1ff..acde4a1a50 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -259,7 +259,7 @@ int test_mixer(int argc, char *argv[]) for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; - if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + if (abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; @@ -333,7 +333,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; From a33700a7ec29221a656af4a1372636e9c2f2ff95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 09:48:29 +0200 Subject: [PATCH 08/16] Actuator controls: Add indices for channels and groups --- msg/actuator_controls.msg | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 414eb06ddb..66e12325d2 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,5 +1,11 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 +uint8 INDEX_FLAPS = 4 +uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control From cde8d72694e342e37ab3ba1879f139c0aa447084 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:02:33 +0200 Subject: [PATCH 09/16] PWM output limiter: Improve comments. --- src/modules/systemlib/pwm_limit/pwm_limit.c | 17 +++++++++++------ src/modules/systemlib/pwm_limit/pwm_limit.h | 9 ++++----- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index adcfb703c0..2f72d347c6 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: Julian Oes + * Copyright (c) 2013-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 @@ -35,9 +34,9 @@ /** * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #include "pwm_limit.h" @@ -46,6 +45,8 @@ #include #include +#define PROGRESS_INT_SCALING 10000 + void pwm_limit_init(pwm_limit_t *limit) { limit->state = PWM_LIMIT_STATE_INIT; @@ -112,7 +113,11 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ { hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = diff * 10000 / RAMP_TIME_US; + progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; + + if (progress > PROGRESS_INT_SCALING) { + progress = PROGRESS_INT_SCALING; + } for (unsigned i=0; i + * Copyright (c) 2013-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 @@ -33,11 +32,11 @@ ****************************************************************************/ /** - * @file pwm_limit.h + * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #ifndef PWM_LIMIT_H_ From 1b4405ee3ac2afe328695cfd28449b1cbe24a606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 09:49:33 +0200 Subject: [PATCH 10/16] FMU driver: Set throttle to zero if in PWM ramp mode --- src/drivers/px4fmu/fmu.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b340694bf0..2047046b9b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -155,7 +155,7 @@ private: pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; - pwm_limit_t _pwm_limit; + static pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -241,6 +241,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; namespace { @@ -272,7 +273,6 @@ PX4FMU::PX4FMU() : _control_subs{-1}, _actuator_output_topic_instance(-1), _poll_fds_num(0), - _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _reverse_pwm_mask(0), @@ -827,6 +827,18 @@ PX4FMU::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; input = controls[control_group].control[control_index]; + + /* motor spinup phase - lock throttle to zero */ + if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + input = 0.0f; + } + } + return 0; } From 6697ffb668ecf74448417d3ca410f97f10276161 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 09:49:52 +0200 Subject: [PATCH 11/16] IO driver: Set throttle to zero if in PWM ramp mode --- src/modules/px4iofirmware/mixer.cpp | 44 +++++++++++++++++++---------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5e6a3a585a..b5d93daea7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -49,6 +49,7 @@ #include #include +#include extern "C" { //#define DEBUG @@ -318,13 +319,6 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE: if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } return -1; @@ -333,13 +327,6 @@ mixer_callback(uintptr_t handle, /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); @@ -353,6 +340,33 @@ mixer_callback(uintptr_t handle, return -1; } + /* apply trim offsets for override channels */ + if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_ROLL) { + control += REG_TO_FLOAT(r_setup_trim_roll); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_PITCH) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_YAW) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } + } + + /* motor spinup phase - lock throttle to zero */ + if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + control = 0.0f; + } + } + /* limit output */ if (control > 1.0f) { control = 1.0f; From 319f9d820f2c77cb8b63cd3342accbd5a183d6de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 12:55:28 +0200 Subject: [PATCH 12/16] telemetry: Crank up rates to make param downloads and other things less painful --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- ROMFS/px4fmu_test/mixers/IO_pass.mix | 24 ++++++++++++++++++++++++ 2 files changed, 28 insertions(+), 4 deletions(-) create mode 100644 ROMFS/px4fmu_test/mixers/IO_pass.mix diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0fb6e117cc..54a469aff8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -457,13 +457,13 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_F "-r 1000 -d /dev/ttyS0" + set MAVLINK_F "-r 5000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_F "-r 1000" + set MAVLINK_F "-r 5000" fi fi @@ -479,11 +479,11 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 60000 -x fi if param compare SYS_COMPANION 157600 then diff --git a/ROMFS/px4fmu_test/mixers/IO_pass.mix b/ROMFS/px4fmu_test/mixers/IO_pass.mix new file mode 100644 index 0000000000..42321f0ad9 --- /dev/null +++ b/ROMFS/px4fmu_test/mixers/IO_pass.mix @@ -0,0 +1,24 @@ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 963972721dc9d709e18f55c2c4adb4e6f141f115 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 13:21:09 +0200 Subject: [PATCH 13/16] MAVLink app: Support rudimentary radio config. --- src/modules/mavlink/mavlink.c | 16 +++++++++ src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 2 ++ 3 files changed, 67 insertions(+) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 460e84c235..62d40d4b74 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,10 +64,26 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); /** +<<<<<<< Updated upstream * MAVLink airframe type * * * @min 0 +======= + * MAVLink Radio ID + * + * When non-zero the MAVLink app will attempt to configure the + * radio to this ID and re-set the parameter to 0. + * + * @group MAVLink + * @min 0 + * @max 240 + */ +PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); + +/** + * MAVLink type +>>>>>>> Stashed changes * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, 1); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 872406775b..3866e86de6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -137,6 +137,7 @@ Mavlink::Mavlink() : _mavlink_ftp(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), + _radio_id(0), _logbuffer {}, _total_counter(0), _receive_thread {}, @@ -170,6 +171,7 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), + _param_radio_id(0), _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), _param_forward_externalsp(0), @@ -489,6 +491,7 @@ void Mavlink::mavlink_update_system(void) if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); + _param_radio_id = param_find("MAV_RADIO_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); _param_forward_externalsp = param_find("MAV_FWDEXTSP"); @@ -504,6 +507,7 @@ void Mavlink::mavlink_update_system(void) int32_t component_id; param_get(_param_component_id, &component_id); + param_get(_param_radio_id, &_radio_id); /* only allow system ID and component ID updates * after reboot - not during operation */ @@ -1508,6 +1512,51 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); } + /* radio config check */ + if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + /* request to configure radio and radio is present */ + FILE *fs = fdopen(_uart_fd, "w"); + + if (fs) { + /* switch to AT command mode */ + usleep(1200000); + fprintf(fs, "+++\n"); + usleep(1200000); + + if (_radio_id > 0) { + /* set channel */ + fprintf(fs, "ATS3=%u\n", _radio_id); + usleep(200000); + } else { + /* reset to factory defaults */ + fprintf(fs, "AT&F\n"); + usleep(200000); + } + + /* write config */ + fprintf(fs, "AT&W"); + usleep(200000); + + /* reboot */ + fprintf(fs, "ATZ"); + usleep(200000); + + warnx("configured radio"); + // XXX NuttX suffers from a bug where + // fclose() also closes the fd, not just + // the file stream. Since this is a one-time + // config thing, we leave the file struct + // allocated. + //fclose(fs); + } else { + warnx("opening %d as file failed", _uart_fd); + } + + /* reset param and save */ + _radio_id = 0; + param_set(_param_radio_id, &_radio_id); + } + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cf95c4d40c..777836f688 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -324,6 +324,7 @@ private: MAVLINK_MODE _mode; mavlink_channel_t _channel; + int32_t _radio_id; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; @@ -381,6 +382,7 @@ private: bool _param_initialized; param_t _param_system_id; param_t _param_component_id; + param_t _param_radio_id; param_t _param_system_type; param_t _param_use_hil_gps; param_t _param_forward_externalsp; From f0e9817f2b2cd2eef15af1c4b6c00cc94fd5d39e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:19:57 +0200 Subject: [PATCH 14/16] ROMFS: Adjust onboard data rate --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 54a469aff8..0a4887a639 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -483,7 +483,7 @@ then fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 60000 -x + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x fi if param compare SYS_COMPANION 157600 then From b8609f99d746bf8926b2ad2fe6dbab95588223c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:24:05 +0200 Subject: [PATCH 15/16] MAVLink app: Fix parameter comments --- src/modules/mavlink/mavlink.c | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 62d40d4b74..b1d369cbf4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -33,9 +33,9 @@ /** * @file mavlink.c - * Adapter functions expected by the protocol library + * Define MAVLink specific parameters * - * @author Lorenz Meier + * @author Lorenz Meier */ #include @@ -46,7 +46,6 @@ #include "mavlink_bridge_header.h" #include -/* define MAVLink specific parameters */ /** * MAVLink system ID * @group MAVLink @@ -64,34 +63,31 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); /** -<<<<<<< Updated upstream - * MAVLink airframe type - * - * - * @min 0 -======= * MAVLink Radio ID * * When non-zero the MAVLink app will attempt to configure the - * radio to this ID and re-set the parameter to 0. + * radio to this ID and re-set the parameter to 0. If the value + * is negative it will reset the complete radio config to + * factory defaults. * * @group MAVLink - * @min 0 + * @min -1 * @max 240 */ PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); /** - * MAVLink type ->>>>>>> Stashed changes + * MAVLink airframe type + * + * @min 1 * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, 1); /** - * Use/Accept HIL GPS message (even if not in HIL mode) + * Use/Accept HIL GPS message even if not in HIL mode * - * If set to 1 incomming HIL GPS messages are parsed. + * If set to 1 incoming HIL GPS messages are parsed. * * @group MAVLink */ @@ -100,8 +96,8 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); /** * Forward external setpoint messages * - * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard - * control mode + * If set to 1 incoming external setpoint messages will be directly forwarded + * to the controllers if in offboard control mode * * @group MAVLink */ From 3ef62121556efeb141b6e3fc36095735aec68654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:26:05 +0200 Subject: [PATCH 16/16] MAVLink app: Less verbose during radio config --- src/modules/mavlink/mavlink_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3866e86de6..1f43031be2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1541,7 +1541,6 @@ Mavlink::task_main(int argc, char *argv[]) fprintf(fs, "ATZ"); usleep(200000); - warnx("configured radio"); // XXX NuttX suffers from a bug where // fclose() also closes the fd, not just // the file stream. Since this is a one-time @@ -1549,7 +1548,7 @@ Mavlink::task_main(int argc, char *argv[]) // allocated. //fclose(fs); } else { - warnx("opening %d as file failed", _uart_fd); + warnx("open fd %d failed", _uart_fd); } /* reset param and save */