From 525e605b926d2ceaf4695e0b49a416ec5cf0a004 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 6 May 2015 12:42:27 +0200 Subject: [PATCH 01/13] vtol: publish actuator controls in manual mode --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 2ae10bd274..fdb4de4343 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -822,7 +822,8 @@ void VtolAttitudeControl::task_main() /* Only publish if the proper mode(s) are enabled */ if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled) + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); From 3ac95fb5816dcbdce4a269767c3f6019c434811f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 10:45:29 +1000 Subject: [PATCH 02/13] lsm303d: run sampling 200usec faster to avoid aliasing this runs the sampling of the accelerometer 200usec faster than requested and then throw away duplicates using the accelerometer status register data ready bit. This avoids aliasing due to drift in the stm32 clock compared to the lsm303d clock --- src/drivers/lsm303d/lsm303d.cpp | 46 +++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e594c92a11..b05df6a6af 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -196,6 +196,7 @@ static const int ERROR = -1; #define REG7_CONT_MODE_M ((0<<1) | (0<<0)) +#define REG_STATUS_A_NEW_ZYXADA 0x08 #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 @@ -217,6 +218,14 @@ static const int ERROR = -1; #define EXTERNAL_BUS 0 #endif +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates using the data ready bit. + This time reduction is enough to cope with worst case timing jitter + due to other timers + */ +#define LSM303D_TIMER_REDUCTION 200 + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -289,9 +298,9 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _accel_reschedules; perf_counter_t _bad_registers; perf_counter_t _bad_values; + perf_counter_t _accel_duplicates; uint8_t _register_wait; @@ -561,9 +570,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")), + _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_accel_duplicates")), _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -622,7 +631,7 @@ LSM303D::~LSM303D() perf_free(_mag_sample_perf); perf_free(_bad_registers); perf_free(_bad_values); - perf_free(_accel_reschedules); + perf_free(_accel_duplicates); } int @@ -874,7 +883,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + _call_accel_interval = ticks; + + _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) @@ -1388,7 +1399,10 @@ LSM303D::start() _mag_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_accel_call, + 1000, + _call_accel_interval - LSM303D_TIMER_REDUCTION, + (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } @@ -1466,20 +1480,6 @@ LSM303D::measure() check_registers(); - // if the accel doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value. - // Note that DRDY is not available when the lsm303d is - // connected on the external bus -#ifdef GPIO_EXTI_ACCEL_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { - perf_count(_accel_reschedules); - hrt_call_delay(&_accel_call, 100); - perf_end(_accel_sample_perf); - return; - } -#endif - if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. @@ -1493,6 +1493,12 @@ LSM303D::measure() raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) { + perf_end(_accel_sample_perf); + perf_count(_accel_duplicates); + return; + } + /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1681,7 +1687,7 @@ LSM303D::print_info() perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); perf_print_counter(_bad_values); - perf_print_counter(_accel_reschedules); + perf_print_counter(_accel_duplicates); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); From a710159263ea5f561d352073504958a9a9f85c81 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 10:50:37 +1000 Subject: [PATCH 03/13] mpu6000: sample at 200usec faster rate to avoid aliasing this runs the mpu6000 200usec faster than requested then detects and disccards duplicates by comparing accel values. This avoids a nasty aliasing issue due to clock drift between the stm32 and mpu6000 --- src/drivers/mpu6000/mpu6000.cpp | 58 ++++++++++++++++++++++++++++++--- 1 file changed, 53 insertions(+), 5 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b6642e2bbd..9880544ecb 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -189,6 +189,14 @@ #define MPU6000_LOW_BUS_SPEED 1000*1000 #define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates by comparing + accelerometer values. This time reduction is enough to cope with + worst case timing jitter due to other timers + */ +#define MPU6000_TIMER_REDUCTION 200 + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -257,6 +265,7 @@ private: perf_counter_t _bad_registers; perf_counter_t _good_transfers; perf_counter_t _reset_retries; + perf_counter_t _duplicates; perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; @@ -287,6 +296,10 @@ private: // last temperature reading for print_info() float _last_temperature; + // keep last accel reading for duplicate detection + uint16_t _last_accel[3]; + bool _got_duplicate; + /** * Start automatic measurement. */ @@ -509,6 +522,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), + _duplicates(perf_alloc(PC_COUNT, "mpu6000_duplicates")), _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), @@ -522,7 +536,9 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _rotation(rotation), _checked_next(0), _in_factory_test(false), - _last_temperature(0) + _last_temperature(0), + _last_accel{}, + _got_duplicate(false) { // disable debug() calls _debug_enabled = false; @@ -576,6 +592,8 @@ MPU6000::~MPU6000() perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_good_transfers); + perf_free(_reset_retries); + perf_free(_duplicates); } int @@ -1198,7 +1216,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu6000 clock + */ + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) @@ -1476,7 +1502,10 @@ MPU6000::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); + hrt_call_every(&_call, + 1000, + _call_interval-MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); } void @@ -1578,14 +1607,32 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - check_registers(); - // sensor transfer at high clock speed set_frequency(MPU6000_HIGH_BUS_SPEED); if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; + check_registers(); + + /* + see if this is duplicate accelerometer data. Note that we + can't use the data ready interrupt status bit in the status + register as that also goes high on new gyro data, and when + we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being + sampled at 8kHz, so we would incorrectly think we have new + data when we are in fact getting duplicate accelerometer data. + */ + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + // it isn't new data - wait for next timer + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); + _got_duplicate = false; + /* * Convert from big to little endian */ @@ -1766,6 +1813,7 @@ MPU6000::print_info() perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); + perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); From db8dd000e3f60a56d63e83b7031ebfeb850f8e04 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 13:26:08 +1000 Subject: [PATCH 04/13] l3gd20: follow same pattern as lsm303d for duplicate rejection --- src/drivers/l3gd20/l3gd20.cpp | 57 +++++++++++++++-------------------- 1 file changed, 24 insertions(+), 33 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7687236408..0319b601e7 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -189,6 +189,14 @@ static const int ERROR = -1; #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates using the data ready bit. + This time reduction is enough to cope with worst case timing jitter + due to other timers + */ +#define L3GD20_TIMER_REDUCTION 200 + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -236,9 +244,9 @@ private: unsigned _read; perf_counter_t _sample_perf; - perf_counter_t _reschedules; perf_counter_t _errors; perf_counter_t _bad_registers; + perf_counter_t _duplicates; uint8_t _register_wait; @@ -410,9 +418,9 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _orientation(SENSOR_BOARD_ROTATION_DEFAULT), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), - _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), + _duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")), _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), @@ -449,9 +457,9 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_reschedules); perf_free(_errors); perf_free(_bad_registers); + perf_free(_duplicates); } int @@ -608,7 +616,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + _call.period = _call_interval - L3GD20_TIMER_REDUCTION; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); @@ -834,7 +844,10 @@ L3GD20::start() _reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); + hrt_call_every(&_call, + 1000, + _call_interval - L3GD20_TIMER_REDUCTION, + (hrt_callout)&L3GD20::measure_trampoline, this); } void @@ -899,12 +912,6 @@ L3GD20::measure_trampoline(void *arg) dev->measure(); } -#ifdef GPIO_EXTI_GYRO_DRDY -# define L3GD20_USE_DRDY 1 -#else -# define L3GD20_USE_DRDY 0 -#endif - void L3GD20::check_registers(void) { @@ -954,33 +961,17 @@ L3GD20::measure() check_registers(); -#if L3GD20_USE_DRDY - // if the gyro doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { - perf_count(_reschedules); - hrt_call_delay(&_call, 100); - perf_end(_sample_perf); - return; - } -#endif - /* fetch data from the sensor */ memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -#if L3GD20_USE_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) { - /* - we waited for DRDY, but did not see DRDY on all axes - when we captured. That means a transfer error of some sort - */ - perf_count(_errors); - return; + if (!(raw_report.status & STATUS_ZYXDA)) { + perf_end(_sample_perf); + perf_count(_duplicates); + return; } -#endif + /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1071,9 +1062,9 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); - perf_print_counter(_reschedules); perf_print_counter(_errors); perf_print_counter(_bad_registers); + perf_print_counter(_duplicates); _reports->print_info("report queue"); ::printf("checked_next: %u\n", _checked_next); for (uint8_t i=0; i Date: Mon, 4 May 2015 22:08:10 +0200 Subject: [PATCH 05/13] Read the current flight termination circuit breaker value, not just updated values. --- src/drivers/px4io/px4io.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0b24ef1526..176f5b241d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -909,6 +909,9 @@ PX4IO::task_main() goto out; } + /* Fetch initial flight termination circuit breaker state */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + /* poll descriptor */ pollfd fds[1]; fds[0].fd = _t_actuator_controls_0; @@ -1079,7 +1082,7 @@ PX4IO::task_main() } } - /* Update Circuit breakers */ + /* Check if the flight termination circuit breaker has been updated */ _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); From 5cf8efcc601466ed455daf41067ad93e79088d83 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 7 May 2015 12:36:50 +0200 Subject: [PATCH 06/13] copy quaternion setpoint into attitude setpoint topic --- .../mc_pos_control/mc_pos_control_main.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 6ffb37d970..0c8b040c1c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1285,6 +1285,11 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; + /* copy quaternion setpoint to attitude setpoint topic */ + math::Quaternion q_sp; + q_sp.from_dcm(R); + memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> euler = R.to_euler(); _att_sp.roll_body = euler(0); @@ -1300,6 +1305,11 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; + /* copy quaternion setpoint to attitude setpoint topic */ + math::Quaternion q_sp; + q_sp.from_dcm(R); + memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; } @@ -1385,6 +1395,11 @@ MulticopterPositionControl::task_main() math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); + + /* copy quaternion setpoint to attitude setpoint topic */ + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); } else { From cc62033190f338ea084e5778cfbc995e1fc47bb9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Apr 2015 12:10:33 +0200 Subject: [PATCH 07/13] improve mavlink set_attitude_target handling port #1920 to mavlink_receiver fixes #1921 --- src/modules/mavlink/mavlink_receiver.cpp | 52 +++++++++++++----------- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 30 insertions(+), 23 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4d96f389db..0cd163a0c9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -129,6 +129,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_alt0(0.0f), _hil_local_proj_ref{}, _offboard_control_mode{}, + _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), _time_offset(0) @@ -778,16 +779,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and * body rates to keep the controllers running */ - bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); - bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; - _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude; } else { - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - _offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg; + _offboard_control_mode.ignore_attitude = ignore_attitude_msg; } _offboard_control_mode.ignore_position = true; @@ -816,22 +817,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(_offboard_control_mode.ignore_attitude)) { - struct vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - mavlink_quaternion_to_euler(set_attitude_target.q, - &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); - att_sp.R_valid = true; - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - att_sp.thrust = set_attitude_target.thrust; + _att_sp.timestamp = hrt_absolute_time(); + if (!ignore_attitude_msg) { // only copy att sp if message contained new data + mavlink_quaternion_to_euler(set_attitude_target.q, + &_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body); + _att_sp.R_valid = true; + _att_sp.yaw_sp_move_rate = 0.0; + memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d)); + _att_sp.q_d_valid = true; } - att_sp.yaw_sp_move_rate = 0.0; - memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); - att_sp.q_d_valid = true; + + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + _att_sp.thrust = set_attitude_target.thrust; + } + if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } } @@ -839,9 +843,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) ///XXX add support for ignoring individual axes if (!(_offboard_control_mode.ignore_bodyrate)) { _rates_sp.timestamp = hrt_absolute_time(); - _rates_sp.roll = set_attitude_target.body_roll_rate; - _rates_sp.pitch = set_attitude_target.body_pitch_rate; - _rates_sp.yaw = set_attitude_target.body_yaw_rate; + if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data + _rates_sp.roll = set_attitude_target.body_roll_rate; + _rates_sp.pitch = set_attitude_target.body_pitch_rate; + _rates_sp.yaw = set_attitude_target.body_yaw_rate; + } if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid _rates_sp.thrust = set_attitude_target.thrust; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 2b6174f8fe..887d2f88ed 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -187,6 +187,7 @@ private: float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; struct offboard_control_mode_s _offboard_control_mode; + struct vehicle_attitude_setpoint_s _att_sp; struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; From 3be1fc7d4271025e5681a2b1898760ca5ef0e930 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 May 2015 11:01:19 +0200 Subject: [PATCH 08/13] commander gyro calibration: Do not require a specific position, automatically start a retry after motion on the first try --- src/modules/commander/gyro_calibration.cpp | 93 ++++++++++++---------- 1 file changed, 53 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index bdef8771e4..fc6398bd68 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -74,7 +74,7 @@ typedef struct { struct gyro_report gyro_report_0; } gyro_worker_data_t; -static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) +static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) { gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); unsigned calibration_counter[max_gyros] = { 0 }; @@ -89,6 +89,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient } memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); + memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale)); /* use first gyro to pace, but count correctly per-gyro for statistics */ while (calibration_counter[0] < calibration_count) { @@ -149,7 +150,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient int do_gyro_calibration(int mavlink_fd) { int res = OK; - gyro_worker_data_t worker_data; + gyro_worker_data_t worker_data = {}; mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -196,51 +197,63 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); } + + int cancel_sub = calibrate_cancel_subscribe(); + + unsigned try_count = 0; + unsigned max_tries = 20; + res = ERROR; - // Calibrate right-side up - - bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false }; - - int cancel_sub = calibrate_cancel_subscribe(); - calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output - cancel_sub, // Subscription to vehicle_command for cancel support - side_collected, // Sides to calibrate - gyro_calibration_worker, // Calibration worker - &worker_data, // Opaque data for calibration worked - true); // true: lenient still detection + do { + // Calibrate gyro and ensure user didn't move + calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data); + + if (cal_return == calibrate_return_cancelled) { + // Cancel message already sent, we are done here + res = ERROR; + break; + + } else if (cal_return == calibrate_return_error) { + res = ERROR; + + } else { + /* check offsets */ + float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; + float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset; + float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.0055f; + + if (!isfinite(worker_data.gyro_scale[0].x_offset) || + !isfinite(worker_data.gyro_scale[0].y_offset) || + !isfinite(worker_data.gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + + mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying.."); + res = ERROR; + + } else { + res = OK; + } + } + try_count++; + + } while (res == ERROR && try_count <= max_tries); + + if (try_count >= max_tries) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration"); + res = ERROR; + } + calibrate_cancel_unsubscribe(cancel_sub); for (unsigned s = 0; s < max_gyros; s++) { close(worker_data.gyro_sensor_sub[s]); } - if (cal_return == calibrate_return_cancelled) { - // Cancel message already sent, we are done here - return ERROR; - } else if (cal_return == calibrate_return_error) { - res = ERROR; - } - - if (res == OK) { - /* check offsets */ - float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; - float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset; - float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; - - /* maximum allowable calibration error in radians */ - const float maxoff = 0.0055f; - - if (!isfinite(worker_data.gyro_scale[0].x_offset) || - !isfinite(worker_data.gyro_scale[0].y_offset) || - !isfinite(worker_data.gyro_scale[0].z_offset) || - fabsf(xdiff) > maxoff || - fabsf(ydiff) > maxoff || - fabsf(zdiff) > maxoff) { - mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration"); - res = ERROR; - } - } - if (res == OK) { /* set offset parameters to new values */ bool failed = false; From 29eeb724a19c2f4e5467df33a092d5eb82d165e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 May 2015 11:23:19 +0200 Subject: [PATCH 09/13] commander: Enforce rotation during mag calibration step --- src/modules/commander/mag_calibration.cpp | 66 ++++++++++++++++++++++- 1 file changed, 65 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 8a8d85818b..1a8de86c41 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -193,7 +194,70 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation"); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); - sleep(2); + + /* + * Detect if the system is rotating. + * + * We're detecting this as a general rotation on any axis, not necessary on the one we + * asked the user for. This is because we really just need two roughly orthogonal axes + * for a good result, so we're not constraining the user more than we have to. + */ + + hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + hrt_abstime last_gyro = 0; + float gyro_x_integral = 0.0f; + float gyro_y_integral = 0.0f; + float gyro_z_integral = 0.0f; + + const float gyro_int_thresh_rad = 1.0f; + + int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + + while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && + fabsf(gyro_y_integral) < gyro_int_thresh_rad && + fabsf(gyro_z_integral) < gyro_int_thresh_rad) { + + /* abort on request */ + if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { + result = calibrate_return_cancelled; + break; + } + + /* abort with timeout */ + if (hrt_absolute_time() > detection_deadline) { + result = calibrate_return_error; + warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); + mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation."); + break; + } + + /* Wait clocking for new data on all gyro */ + struct pollfd fds[1]; + fds[0].fd = sub_gyro; + fds[0].events = POLLIN; + size_t fd_count = 1; + + int poll_ret = poll(fds, fd_count, 1000); + + if (poll_ret > 0) { + struct gyro_report gyro; + orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); + + /* ensure we have a valid first timestamp */ + if (last_gyro > 0) { + + /* integrate */ + float delta_t = (gyro.timestamp - last_gyro) / 1e6f; + gyro_x_integral += gyro.x * delta_t; + gyro_y_integral += gyro.y * delta_t; + gyro_z_integral += gyro.z * delta_t; + } + + last_gyro = gyro.timestamp; + } + } + + close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; From b6441d29661be76d6790a4c492114065424174a3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 11 Apr 2015 19:58:53 +0200 Subject: [PATCH 10/13] implemented new mixer strategy --- .../systemlib/mixer/mixer_multirotor.cpp | 127 ++++++++++++------ 1 file changed, 85 insertions(+), 42 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index e9a8a87ca8..4d9d60c2d8 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -200,18 +200,31 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl unsigned MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { + /* Summary of mixing strategy: + 1) mix roll, pitch and thrust without yaw. + 2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation -> + increase or decrease total thrust (boost). The total increase or decrease of thrust is limited + (max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch. + In case there is violation at the lower and upper bound then try to shift such that violation is equal + on both sides. + 3) mix in yaw and scale if it leads to limit violation. + 4) scale all outputs to range [idle_speed,1] + */ + float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); - //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); float thrust = constrain(get_control(0, 3), 0.0f, 1.0f); - //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; + // clean register for saturation status flags if (status_reg != NULL) { (*status_reg) = 0; } + // thrust boost parameters + float thrust_increase_factor = 1.5f; + float thrust_decrease_factor = 0.6f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -221,14 +234,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) out *= _rotors[i].out_scale; - /* limit yaw if it causes outputs clipping */ - if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { - yaw = -out / _rotors[i].yaw_scale; - if(status_reg != NULL) { - (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; - } - } - /* calculate min and max output values */ if (out < min_out) { min_out = out; @@ -240,51 +245,89 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) outputs[i] = out; } - /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0f) { - float scale_in = thrust / (thrust - min_out); + float boost = 0.0f; // value added to demanded thrust (can also be negative) + float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch - max_out = 0.0f; - - /* mix again with adjusted controls */ - for (unsigned i = 0; i < _rotor_count; i++) { - float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; - - /* update max output value */ - if (out > max_out) { - max_out = out; - } - - outputs[i] = out; + if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + float max_thrust_diff = thrust * thrust_increase_factor - thrust; + if(max_thrust_diff >= -min_out) { + boost = -min_out; } + else { + boost = max_thrust_diff; + roll_pitch_scale = (thrust + boost)/(thrust - min_out); + } + } + else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor*thrust; + if(max_thrust_diff >= max_out - 1.0f) { + boost = -(max_out - 1.0f); + } else { + boost = -max_thrust_diff; + roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + } + } + else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + float max_thrust_diff = thrust * thrust_increase_factor - thrust; + boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff); + roll_pitch_scale = (thrust + boost)/(thrust - min_out); + } + else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) { + float max_thrust_diff = thrust - thrust_decrease_factor*thrust; + boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f); + roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + } + else if (min_out < 0.0f && max_out > 1.0f) { + boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust); + roll_pitch_scale = (thrust + boost)/(thrust - min_out); + } + // notify if saturation has occurred + if(min_out < 0.0f) { if(status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } - - } else { - /* roll/pitch mixed without lower side limiting, add yaw control */ - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += yaw * _rotors[i].yaw_scale; - } } - - /* scale down all outputs if some outputs are too large, reduce total thrust */ - float scale_out; - if (max_out > 1.0f) { - scale_out = 1.0f / max_out; - + if(max_out > 0.0f) { if(status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; } - - } else { - scale_out = 1.0f; } - /* scale outputs to range _idle_speed..1, and do final limiting */ + // mix again but now with thrust boost, scale roll/pitch and also add yaw + for(unsigned i = 0; i < _rotor_count; i++) { + float out = (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; + + out *= _rotors[i].out_scale; + + // scale yaw if it violates limits. inform about yaw limit reached + if(out < 0.0f) { + yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } + } + else if(out > 1.0f) { + yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } + } + } + + /* last mix, add yaw and scale outputs to range idle_speed...1 */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); + outputs[i] = (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; + + outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } return _rotor_count; From 7f3f572c406517cf1b5cb713c691343f2a5fc189 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 May 2015 10:21:16 +0200 Subject: [PATCH 11/13] Always boot, even with no pressure sensor or ADC. Fixes #2151 --- ROMFS/px4fmu_common/init.d/rc.sensors | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index b8a704b90f..474db36ef7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -3,8 +3,13 @@ # Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers. # -ms5611 start -adc start +if ms5611 start +then +fi + +if adc start +then +fi if ver hwcmp PX4FMU_V2 then From a124bc07aa170b7ea03a901e480157631396d5cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 May 2015 23:58:25 +0200 Subject: [PATCH 12/13] Ensure SYS_AUTOSTART param is always selected --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4fbc0416e5..153e704809 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -633,6 +633,7 @@ Sensors::Sensors() : (void)param_find("CAL_MAG1_ROT"); (void)param_find("CAL_MAG2_ROT"); (void)param_find("SYS_PARAM_VER"); + (void)param_find("SYS_AUTOSTART"); /* fetch initial parameter values */ parameters_update(); From b3ed0cf36b985771c50108016277f2148c8b000f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 May 2015 09:37:42 +0200 Subject: [PATCH 13/13] Navigator: be more verbose about RTL --- src/modules/navigator/rtl.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c1ed8cb7c1..b75b7fa221 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -103,6 +103,7 @@ RTL::on_activation() _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; } + } set_rtl_item(); @@ -146,7 +147,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)", + (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt)); break; } @@ -177,7 +179,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)", + (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } @@ -197,7 +200,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)", + (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; }