From 3ac95fb5816dcbdce4a269767c3f6019c434811f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 10:45:29 +1000 Subject: [PATCH 01/41] 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 02/41] 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 03/41] 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 04/41] 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 05/41] 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 1d544e028d6041130bd673b69d74e04ea9357547 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 7 May 2015 15:04:59 +0200 Subject: [PATCH 06/41] mc_pos_control: always update previous velocity to avoid spikes due to differentiation --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +++- 1 file changed, 3 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 0c8b040c1c..e5ea4d1047 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1102,7 +1102,6 @@ MulticopterPositionControl::task_main() /* derivative of velocity error, not includes setpoint acceleration */ math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; - _vel_prev = _vel; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; @@ -1406,6 +1405,9 @@ MulticopterPositionControl::task_main() reset_yaw_sp = true; } + /* update previous velocity for velocity controller D part */ + _vel_prev = _vel; + /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, * in this case the attitude setpoint is published by the mavlink app From cc62033190f338ea084e5778cfbc995e1fc47bb9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Apr 2015 12:10:33 +0200 Subject: [PATCH 07/41] 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/41] 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/41] 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/41] 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/41] 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/41] 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/41] 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; } From e400ec586d0925fc3137637a78b5156f499cfd29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 15:18:50 +0200 Subject: [PATCH 14/41] multiplatform att controller: Rename params to prevent mixup with MC params --- .../mc_att_control.cpp | 34 +++++++++--------- .../mc_att_control_params.c | 34 +++++++++--------- .../mc_att_control_params.h | 36 +++++++++---------- 3 files changed, 52 insertions(+), 52 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index b5a551109f..9e9c8ad5ff 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -75,23 +75,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* parameters */ _params_handles({ - .roll_p = px4::ParameterFloat("MC_ROLL_P", PARAM_MC_ROLL_P_DEFAULT), - .roll_rate_p = px4::ParameterFloat("MC_ROLLRATE_P", PARAM_MC_ROLLRATE_P_DEFAULT), - .roll_rate_i = px4::ParameterFloat("MC_ROLLRATE_I", PARAM_MC_ROLLRATE_I_DEFAULT), - .roll_rate_d = px4::ParameterFloat("MC_ROLLRATE_D", PARAM_MC_ROLLRATE_D_DEFAULT), - .pitch_p = px4::ParameterFloat("MC_PITCH_P", PARAM_MC_PITCH_P_DEFAULT), - .pitch_rate_p = px4::ParameterFloat("MC_PITCHRATE_P", PARAM_MC_PITCHRATE_P_DEFAULT), - .pitch_rate_i = px4::ParameterFloat("MC_PITCHRATE_I", PARAM_MC_PITCHRATE_I_DEFAULT), - .pitch_rate_d = px4::ParameterFloat("MC_PITCHRATE_D", PARAM_MC_PITCHRATE_D_DEFAULT), - .yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT), - .yaw_rate_p = px4::ParameterFloat("MC_YAWRATE_P", PARAM_MC_YAWRATE_P_DEFAULT), - .yaw_rate_i = px4::ParameterFloat("MC_YAWRATE_I", PARAM_MC_YAWRATE_I_DEFAULT), - .yaw_rate_d = px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT), - .yaw_ff = px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT), - .yaw_rate_max = px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT), - .acro_roll_max = px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT), - .acro_pitch_max = px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT), - .acro_yaw_max = px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT) + .roll_p = px4::ParameterFloat("MP_ROLL_P", PARAM_MP_ROLL_P_DEFAULT), + .roll_rate_p = px4::ParameterFloat("MP_ROLLRATE_P", PARAM_MP_ROLLRATE_P_DEFAULT), + .roll_rate_i = px4::ParameterFloat("MP_ROLLRATE_I", PARAM_MP_ROLLRATE_I_DEFAULT), + .roll_rate_d = px4::ParameterFloat("MP_ROLLRATE_D", PARAM_MP_ROLLRATE_D_DEFAULT), + .pitch_p = px4::ParameterFloat("MP_PITCH_P", PARAM_MP_PITCH_P_DEFAULT), + .pitch_rate_p = px4::ParameterFloat("MP_PITCHRATE_P", PARAM_MP_PITCHRATE_P_DEFAULT), + .pitch_rate_i = px4::ParameterFloat("MP_PITCHRATE_I", PARAM_MP_PITCHRATE_I_DEFAULT), + .pitch_rate_d = px4::ParameterFloat("MP_PITCHRATE_D", PARAM_MP_PITCHRATE_D_DEFAULT), + .yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT), + .yaw_rate_p = px4::ParameterFloat("MP_YAWRATE_P", PARAM_MP_YAWRATE_P_DEFAULT), + .yaw_rate_i = px4::ParameterFloat("MP_YAWRATE_I", PARAM_MP_YAWRATE_I_DEFAULT), + .yaw_rate_d = px4::ParameterFloat("MP_YAWRATE_D", PARAM_MP_YAWRATE_D_DEFAULT), + .yaw_ff = px4::ParameterFloat("MP_YAW_FF", PARAM_MP_YAW_FF_DEFAULT), + .yaw_rate_max = px4::ParameterFloat("MP_YAWRATE_MAX", PARAM_MP_YAWRATE_MAX_DEFAULT), + .acro_roll_max = px4::ParameterFloat("MP_ACRO_R_MAX", PARAM_MP_ACRO_R_MAX_DEFAULT), + .acro_pitch_max = px4::ParameterFloat("MP_ACRO_P_MAX", PARAM_MP_ACRO_P_MAX_DEFAULT), + .acro_yaw_max = px4::ParameterFloat("MP_ACRO_Y_MAX", PARAM_MP_ACRO_Y_MAX_DEFAULT) }), /* performance counters */ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c index 395ae83b02..9c6ac5dc73 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -52,7 +52,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); +PX4_PARAM_DEFINE_FLOAT(MP_ROLL_P); /** * Roll rate P gain @@ -62,7 +62,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); +PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_P); /** * Roll rate I gain @@ -72,7 +72,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); +PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_I); /** * Roll rate D gain @@ -82,7 +82,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); +PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_D); /** * Pitch P gain @@ -93,7 +93,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); +PX4_PARAM_DEFINE_FLOAT(MP_PITCH_P); /** * Pitch rate P gain @@ -103,7 +103,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); +PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_P); /** * Pitch rate I gain @@ -113,7 +113,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); +PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_I); /** * Pitch rate D gain @@ -123,7 +123,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); +PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_D); /** * Yaw P gain @@ -134,7 +134,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); +PX4_PARAM_DEFINE_FLOAT(MP_YAW_P); /** * Yaw rate P gain @@ -144,7 +144,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); +PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_P); /** * Yaw rate I gain @@ -154,7 +154,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); +PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_I); /** * Yaw rate D gain @@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); +PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_D); /** * Yaw feed forward @@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); * @max 1.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); +PX4_PARAM_DEFINE_FLOAT(MP_YAW_FF); /** * Max yaw rate @@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); * @max 360.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); +PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_MAX); /** * Max acro roll rate @@ -197,7 +197,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); * @max 360.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); +PX4_PARAM_DEFINE_FLOAT(MP_ACRO_R_MAX); /** * Max acro pitch rate @@ -207,7 +207,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); * @max 360.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); +PX4_PARAM_DEFINE_FLOAT(MP_ACRO_P_MAX); /** * Max acro yaw rate @@ -216,4 +216,4 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX); +PX4_PARAM_DEFINE_FLOAT(MP_ACRO_Y_MAX); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h index ff962dbf1f..c3b71715bb 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file mc_att_control_params.h + * @file MP_att_control_params.h * Parameters for multicopter attitude controller. * * @author Tobias Naegeli @@ -43,20 +43,20 @@ */ #pragma once -#define PARAM_MC_ROLL_P_DEFAULT 6.0f -#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f -#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f -#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f -#define PARAM_MC_PITCH_P_DEFAULT 6.0f -#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f -#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f -#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f -#define PARAM_MC_YAW_P_DEFAULT 2.0f -#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f -#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f -#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f -#define PARAM_MC_YAW_FF_DEFAULT 0.5f -#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f -#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f -#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f -#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f +#define PARAM_MP_ROLL_P_DEFAULT 6.0f +#define PARAM_MP_ROLLRATE_P_DEFAULT 0.1f +#define PARAM_MP_ROLLRATE_I_DEFAULT 0.0f +#define PARAM_MP_ROLLRATE_D_DEFAULT 0.002f +#define PARAM_MP_PITCH_P_DEFAULT 6.0f +#define PARAM_MP_PITCHRATE_P_DEFAULT 0.1f +#define PARAM_MP_PITCHRATE_I_DEFAULT 0.0f +#define PARAM_MP_PITCHRATE_D_DEFAULT 0.002f +#define PARAM_MP_YAW_P_DEFAULT 2.0f +#define PARAM_MP_YAWRATE_P_DEFAULT 0.3f +#define PARAM_MP_YAWRATE_I_DEFAULT 0.0f +#define PARAM_MP_YAWRATE_D_DEFAULT 0.0f +#define PARAM_MP_YAW_FF_DEFAULT 0.5f +#define PARAM_MP_YAWRATE_MAX_DEFAULT 120.0f +#define PARAM_MP_ACRO_R_MAX_DEFAULT 35.0f +#define PARAM_MP_ACRO_P_MAX_DEFAULT 35.0f +#define PARAM_MP_ACRO_Y_MAX_DEFAULT 120.0f From efb60ab7794579d4cf1b2684c30fb4a13108c0fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 15:19:16 +0200 Subject: [PATCH 15/41] multiplatform pos controller: Rename params to prevent mixup with MPC params --- .../mc_pos_control.cpp | 40 +++++++++---------- .../mc_pos_control_params.c | 40 +++++++++---------- .../mc_pos_control_params.h | 40 +++++++++---------- 3 files changed, 60 insertions(+), 60 deletions(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index d135eecfbb..90281d2bc9 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -69,26 +69,26 @@ MulticopterPositionControl::MulticopterPositionControl() : /* parameters */ _params_handles({ - .thr_min = px4::ParameterFloat("MPC_THR_MIN", PARAM_MPC_THR_MIN_DEFAULT), - .thr_max = px4::ParameterFloat("MPC_THR_MAX", PARAM_MPC_THR_MAX_DEFAULT), - .z_p = px4::ParameterFloat("MPC_Z_P", PARAM_MPC_Z_P_DEFAULT), - .z_vel_p = px4::ParameterFloat("MPC_Z_VEL_P", PARAM_MPC_Z_VEL_P_DEFAULT), - .z_vel_i = px4::ParameterFloat("MPC_Z_VEL_I", PARAM_MPC_Z_VEL_I_DEFAULT), - .z_vel_d = px4::ParameterFloat("MPC_Z_VEL_D", PARAM_MPC_Z_VEL_D_DEFAULT), - .z_vel_max = px4::ParameterFloat("MPC_Z_VEL_MAX", PARAM_MPC_Z_VEL_MAX_DEFAULT), - .z_ff = px4::ParameterFloat("MPC_Z_FF", PARAM_MPC_Z_FF_DEFAULT), - .xy_p = px4::ParameterFloat("MPC_XY_P", PARAM_MPC_XY_P_DEFAULT), - .xy_vel_p = px4::ParameterFloat("MPC_XY_VEL_P", PARAM_MPC_XY_VEL_P_DEFAULT), - .xy_vel_i = px4::ParameterFloat("MPC_XY_VEL_I", PARAM_MPC_XY_VEL_I_DEFAULT), - .xy_vel_d = px4::ParameterFloat("MPC_XY_VEL_D", PARAM_MPC_XY_VEL_D_DEFAULT), - .xy_vel_max = px4::ParameterFloat("MPC_XY_VEL_MAX", PARAM_MPC_XY_VEL_MAX_DEFAULT), - .xy_ff = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT), - .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), - .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), - .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), - .man_roll_max = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), - .man_pitch_max = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), - .man_yaw_max = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT), + .thr_min = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT), + .thr_max = px4::ParameterFloat("MPP_THR_MAX", PARAM_MPP_THR_MAX_DEFAULT), + .z_p = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT), + .z_vel_p = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT), + .z_vel_i = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT), + .z_vel_d = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT), + .z_vel_max = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT), + .z_ff = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT), + .xy_p = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT), + .xy_vel_p = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT), + .xy_vel_i = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT), + .xy_vel_d = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT), + .xy_vel_max = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT), + .xy_ff = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT), + .tilt_max_air = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT), + .land_speed = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT), + .tilt_max_land = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT), + .man_roll_max = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT), .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT) }), _ref_alt(0.0f), diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c index 709085662a..1b3c5b7e93 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -52,7 +52,7 @@ * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); +PX4_PARAM_DEFINE_FLOAT(MPP_THR_MIN); /** * Maximum thrust @@ -63,7 +63,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_THR_MAX); /** * Proportional gain for vertical position error @@ -71,7 +71,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_P); /** * Proportional gain for vertical velocity error @@ -79,7 +79,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_P); /** * Integral gain for vertical velocity error @@ -89,7 +89,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_I); /** * Differential gain for vertical velocity error @@ -97,7 +97,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_D); /** * Maximum vertical velocity @@ -108,7 +108,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_MAX); /** * Vertical velocity feed forward @@ -119,7 +119,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); +PX4_PARAM_DEFINE_FLOAT(MPP_Z_FF); /** * Proportional gain for horizontal position error @@ -127,7 +127,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_P); /** * Proportional gain for horizontal velocity error @@ -135,7 +135,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_P); /** * Integral gain for horizontal velocity error @@ -145,7 +145,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_I); /** * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. @@ -153,7 +153,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_D); /** * Maximum horizontal velocity @@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_MAX); /** * Horizontal velocity feed forward @@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); * @max 1.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); +PX4_PARAM_DEFINE_FLOAT(MPP_XY_FF); /** * Maximum tilt angle in air @@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); * @max 90.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); +PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_AIR); /** * Maximum tilt during landing @@ -199,7 +199,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); * @max 90.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); +PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_LND); /** * Landing descend rate @@ -208,7 +208,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); * @min 0.0 * @group Multicopter Position Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); +PX4_PARAM_DEFINE_FLOAT(MPP_LAND_SPEED); /** * Max manual roll @@ -218,7 +218,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); * @max 90.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_MAN_R_MAX); /** * Max manual pitch @@ -228,7 +228,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX); * @max 90.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_MAN_P_MAX); /** * Max manual yaw rate @@ -237,5 +237,5 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX); * @min 0.0 * @group Multicopter Attitude Control */ -PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX); +PX4_PARAM_DEFINE_FLOAT(MPP_MAN_Y_MAX); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h index 8c8b707ae0..d9c9fb5957 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -41,24 +41,24 @@ #pragma once -#define PARAM_MPC_THR_MIN_DEFAULT 0.1f -#define PARAM_MPC_THR_MAX_DEFAULT 1.0f -#define PARAM_MPC_Z_P_DEFAULT 1.0f -#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f -#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f -#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f -#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f -#define PARAM_MPC_Z_FF_DEFAULT 0.5f -#define PARAM_MPC_XY_P_DEFAULT 1.0f -#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f -#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f -#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f -#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f -#define PARAM_MPC_XY_FF_DEFAULT 0.5f -#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f -#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f -#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f -#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f -#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f -#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f +#define PARAM_MPP_THR_MIN_DEFAULT 0.1f +#define PARAM_MPP_THR_MAX_DEFAULT 1.0f +#define PARAM_MPP_Z_P_DEFAULT 1.0f +#define PARAM_MPP_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPP_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPP_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPP_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPP_Z_FF_DEFAULT 0.5f +#define PARAM_MPP_XY_P_DEFAULT 1.0f +#define PARAM_MPP_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPP_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPP_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPP_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPP_XY_FF_DEFAULT 0.5f +#define PARAM_MPP_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPP_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPP_LAND_SPEED_DEFAULT 1.0f +#define PARAM_MPP_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MPP_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MPP_MAN_Y_MAX_DEFAULT 120.0f From 542dc9f65d05291b8b9661374044f8c51a4dd209 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 13 May 2015 15:28:02 +0200 Subject: [PATCH 16/41] added default rate I gain for multicopters. added default gains for FireFly6 in mc mode (from Simon Wilks) --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 ++-- .../init.d/10017_steadidrone_qu4d | 4 ++-- ROMFS/px4fmu_common/init.d/13002_firefly6 | 20 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 4 ++-- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 6 +++--- 7 files changed, 33 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index caa8e28fc5..68428be6f6 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -12,11 +12,11 @@ then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.5 param set MC_YAWRATE_P 0.25 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 5e41d6957d..8ab65be7b9 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -12,11 +12,11 @@ then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 0.5 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 191c58da4c..ed90dabf41 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -7,6 +7,26 @@ sh /etc/init.d/rc.vtol_defaults +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + set MIXER firefly6 set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 8e5dc76b1f..d07e926a79 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -11,11 +11,11 @@ if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ea35b3ba9b..9b3954be6f 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -12,11 +12,11 @@ then # TODO REVIEW param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index b1db1dd9aa..05b4355138 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -12,11 +12,11 @@ then # TODO REVIEW param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index a1de19d5df..aec0c62f8f 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -13,15 +13,15 @@ if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 890c3af13c397d6de049e21509bb2a7002c5b9da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 15:34:07 +0200 Subject: [PATCH 17/41] multiplatform: Update yaw P param name --- src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 90281d2bc9..e0e086999d 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -89,7 +89,7 @@ MulticopterPositionControl::MulticopterPositionControl() : .man_roll_max = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT), .man_pitch_max = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT), .man_yaw_max = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT), - .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT) + .mc_att_yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT) }), _ref_alt(0.0f), _ref_timestamp(0), From 6f949510501c34c3a114db7fdb32d6fb8d47c36a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 15:46:35 +0200 Subject: [PATCH 18/41] Fix sdlog2 stopping on param write. Found by Severin Leuenberger --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ae49135592..c76569cf8f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1975,10 +1975,10 @@ void handle_command(struct vehicle_command_s *cmd) if (param == 1) { sdlog2_start_log(); - } else if (param == 0) { + } else if (param == -1) { sdlog2_stop_log(); } else { - warnx("unknown storage cmd"); + // Silently ignore non-matching command values, as they could be for params. } break; From 66b87ac7618d2e886b5389e3eef50af182b32f25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 16:04:33 +0200 Subject: [PATCH 19/41] Fix state-level HIL. Reported by @s3erjaeh --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0cd163a0c9..988232dc21 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1360,8 +1360,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; + hil_global_pos.lat = hil_state.lat / ((double)1e7); + hil_global_pos.lon = hil_state.lon / ((double)1e7); hil_global_pos.alt = hil_state.alt / 1000.0f; hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_e = hil_state.vy / 100.0f; From 90c3aec1fdc640ef15a610991251fa8a6ab202e8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 13 May 2015 16:23:29 +0200 Subject: [PATCH 20/41] make main LED show low and critical battery status --- src/modules/commander/commander.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50846ff4d0..f274879325 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2157,10 +2157,12 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { + if (status_local->failsafe) { + rgbled_set_color(RGBLED_COLOR_PURPLE); + } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); - /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ - + } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + rgbled_set_color(RGBLED_COLOR_RED); } else { if (status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); From 16aa03e30a8844b03b9594bbf2008ff6151db4eb Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Wed, 13 May 2015 17:22:10 +0200 Subject: [PATCH 21/41] mpc parameter INAV_W_Z_VIS_P increased --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 96b12f9e09..382e9e46d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); /** * Z axis weight for sonar From b4e7b041cac7a937df3ea4e84dc943bff809fb55 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Wed, 13 May 2015 11:19:29 -0400 Subject: [PATCH 22/41] Fix potential null pointer deref if Mavlink start fails before task_main loop LL_APPEND is called just before the loop spins up but various error conditions can cause the task to exit before then. When that happens Mavlink::start_helper calls delete on the instance which tries to prune it from the global list. If this is the first Mavlink instance to attempt starting the list head is null and we hardfault in the Mavlink dtor. Only call LL_DELETE after checking the list head for a null pointer. --- src/modules/mavlink/mavlink_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 326b0b5ab4..a76abc298d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -249,7 +249,9 @@ Mavlink::~Mavlink() } while (_task_running); } - LL_DELETE(_mavlink_instances, this); + if (_mavlink_instances) { + LL_DELETE(_mavlink_instances, this); + } } void From b0efb24468564850e1fdb9bf7692fd4e2b939235 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Wed, 13 May 2015 22:21:59 +0200 Subject: [PATCH 23/41] add USART6 on PC6(TX) and PC7(RX) to discovery --- .../px4-stm32f4discovery/include/board.h | 18 ++++++++-- .../px4-stm32f4discovery/nsh/defconfig | 35 +++++++++++++------ 2 files changed, 40 insertions(+), 13 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h index 0412c3aa6d..432d574092 100644 --- a/nuttx-configs/px4-stm32f4discovery/include/board.h +++ b/nuttx-configs/px4-stm32f4discovery/include/board.h @@ -34,8 +34,8 @@ * ************************************************************************************/ -#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H -#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H /************************************************************************************ * Included Files @@ -200,6 +200,18 @@ #define GPIO_USART2_RX GPIO_USART2_RX_1 #define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* UART6: + * + * The STM32F4 Discovery has no on-board serial devices, PC6 (TX) and PC7 (RX) + * for connection to an external serial device. + */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* USART6 require a RX DMA configuration */ +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 /* SPI - There is a MEMS device on SPI1 using these pins: */ @@ -270,4 +282,4 @@ EXTERN void stm32_setleds(uint8_t ledset); #endif #endif /* __ASSEMBLY__ */ -#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig index 6a2470bea9..03092256e9 100644 --- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -91,6 +91,8 @@ CONFIG_ARCH_HAVE_MPU=y # ARMV7M Configuration Options # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y @@ -128,6 +130,7 @@ CONFIG_SERIAL_TERMIOS=y # CONFIG_ARCH_CHIP_STM32F100VC is not set # CONFIG_ARCH_CHIP_STM32F100VD is not set # CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set # CONFIG_ARCH_CHIP_STM32F103C4 is not set # CONFIG_ARCH_CHIP_STM32F103C8 is not set # CONFIG_ARCH_CHIP_STM32F103RET6 is not set @@ -173,7 +176,6 @@ CONFIG_ARCH_CHIP_STM32F407VG=y # CONFIG_STM32_STM32F20XX is not set # CONFIG_STM32_STM32F30XX is not set CONFIG_STM32_STM32F40XX=y -# CONFIG_STM32_STM32F427 is not set # CONFIG_STM32_DFU is not set # @@ -207,9 +209,6 @@ CONFIG_STM32_PWR=y CONFIG_STM32_SPI1=y # CONFIG_STM32_SPI2 is not set # CONFIG_STM32_SPI3 is not set -# CONFIG_STM32_SPI4 is not set -# CONFIG_STM32_SPI5 is not set -# CONFIG_STM32_SPI6 is not set CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM1=y # CONFIG_STM32_TIM2 is not set @@ -230,9 +229,7 @@ CONFIG_STM32_USART2=y # CONFIG_STM32_USART3 is not set # CONFIG_STM32_UART4 is not set # CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set -# CONFIG_STM32_UART7 is not set -# CONFIG_STM32_UART8 is not set +CONFIG_STM32_USART6=y # CONFIG_STM32_IWDG is not set # CONFIG_STM32_WWDG is not set CONFIG_STM32_ADC=y @@ -268,6 +265,8 @@ CONFIG_STM32_USART=y # # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_USART_SINGLEWIRE=y @@ -336,8 +335,10 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD="" +CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y +# CONFIG_ARCH_BOARD_MIKROE_STM32F4 is not set +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="stm32f4discovery" # # Common Board Options @@ -431,7 +432,7 @@ CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set -# CONFIG_RTC is not set +# CONFIG_RTC= is not set CONFIG_WATCHDOG=y # CONFIG_ANALOG is not set # CONFIG_AUDIO_DEVICES is not set @@ -449,10 +450,12 @@ CONFIG_SERIAL=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set # @@ -466,6 +469,18 @@ CONFIG_USART2_PARITY=0 CONFIG_USART2_2STOP=0 # CONFIG_USART2_IFLOWCONTROL is not set # CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set # CONFIG_SERIAL_IFLOWCONTROL is not set # CONFIG_SERIAL_OFLOWCONTROL is not set CONFIG_USBDEV=y From 52fca5b7a153e21970d9e8f62e24d76a1ca49cf5 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Wed, 13 May 2015 22:24:32 +0200 Subject: [PATCH 24/41] correct comments --- src/drivers/boards/px4-stm32f4discovery/board_config.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index dd66abc19f..c4f469caa1 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -60,9 +60,10 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ -/* PX4FMU GPIOs ***********************************************************************************/ -/* LEDs */ +/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/ /* LEDs */ +// LED1 green, LED2 orange, LED3 red, LED4 blue + #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) From 169f112f0d1b883b26e7d8297a4d898cf709cf06 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Wed, 13 May 2015 22:59:30 +0200 Subject: [PATCH 25/41] change name in board.h --- nuttx-configs/px4-stm32f4discovery/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h index 432d574092..95feca5506 100644 --- a/nuttx-configs/px4-stm32f4discovery/include/board.h +++ b/nuttx-configs/px4-stm32f4discovery/include/board.h @@ -34,8 +34,8 @@ * ************************************************************************************/ -#ifndef __ARCH_BOARD_BOARD_H -#define __ARCH_BOARD_BOARD_H +#ifndef __CONFIG_DISCOVERY_INCLUDE_BOARD_H +#define __CONFIG_DISCOVERY_INCLUDE_BOARD_H /************************************************************************************ * Included Files @@ -282,4 +282,4 @@ EXTERN void stm32_setleds(uint8_t ledset); #endif #endif /* __ASSEMBLY__ */ -#endif /* __ARCH_BOARD_BOARD_H */ +#endif /* __CONFIG_DISCOVERY_INCLUDE_BOARD_H */ From 8ac81515ce30100cda801134ad7c4cd9a9be6bd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:11:36 +0200 Subject: [PATCH 26/41] EKF estimator: Remove unused params --- .../attitude_estimator_ekf_main.cpp | 13 +------------ .../attitude_estimator_ekf_params.c | 14 -------------- 2 files changed, 1 insertion(+), 26 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 3faf63a27f..fb7cda8c42 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -397,16 +397,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) hrt_abstime vel_t = 0; bool vel_valid = false; - if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { - vel_valid = true; - if (gps_updated) { - vel_t = gps.timestamp_velocity; - vel(0) = gps.vel_n_m_s; - vel(1) = gps.vel_e_m_s; - vel(2) = gps.vel_d_m_s; - } - - } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; @@ -487,8 +478,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); - } else { - mag_decl = ekf_params.mag_decl; } /* update mag declination rotation matrix */ 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 e981c6eb74..13a9fa5f6f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -107,11 +107,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); */ PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1); -/* magnetic declination, in degrees */ -PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); - -PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); - /** * Moment of inertia matrix diagonal entry (1, 1) * @@ -159,10 +154,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r1 = param_find("EKF_ATT_V4_R1"); h->r2 = param_find("EKF_ATT_V4_R2"); - h->mag_decl = param_find("ATT_MAG_DECL"); - - h->acc_comp = param_find("ATT_ACC_COMP"); - h->moment_inertia_J[0] = param_find("ATT_J11"); h->moment_inertia_J[1] = param_find("ATT_J22"); h->moment_inertia_J[2] = param_find("ATT_J33"); @@ -182,11 +173,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); - param_get(h->mag_decl, &(p->mag_decl)); - p->mag_decl *= M_PI_F / 180.0f; - - param_get(h->acc_comp, &(p->acc_comp)); - for (int i = 0; i < 3; i++) { param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); } From 33338ee2d0523867088c534bf0428ec1a60de950 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:11:56 +0200 Subject: [PATCH 27/41] Q attitude estimator: Comment undocumented params --- .../attitude_estimator_q_params.c | 68 +++++++++++++++++-- 1 file changed, 64 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index fa15923076..730d7a4e8b 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -41,10 +41,70 @@ #include +/** + * Complimentary filter accelerometer weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); + +/** + * Complimentary filter magnetometer weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); + +/** + * Complimentary filter gyroscope bias weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ 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 + +/** + * Magnetic declination, in degrees + * + * This parameter is not used in normal operation, + * as the declination is looked up based on the + * GPS coordinates of the vehicle. + * + * @group Attitude Q estimator + * @unit degrees + */ +PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); + +/** + * Enable automatic GPS based declination compensation + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); + +/** + * Enable acceleration compensation based on GPS + * velocity. + * + * @group Attitude Q estimator + * @min 1 + * @max 2 + */ +PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); + +/** + * Gyro bias limit + * + * @group Attitude Q estimator + * @min 0 + * @max 2 + * @unit rad/s + */ +PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); From e7c30322187068d76cf6ef9ea9935856134a3c6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:31:05 +0200 Subject: [PATCH 28/41] Mag calibration: Accept less rotation to start the UI, ensure calibration abort does indeed abort. --- src/modules/commander/mag_calibration.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1a8de86c41..04e66a5cb6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -209,7 +209,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta float gyro_y_integral = 0.0f; float gyro_z_integral = 0.0f; - const float gyro_int_thresh_rad = 1.0f; + const float gyro_int_thresh_rad = 0.5f; int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); @@ -220,7 +220,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; - break; + close(sub_gyro); + return result; } /* abort with timeout */ From 8a73f91ad6fb18c451f04cfe7a976b66f806031a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:33:07 +0200 Subject: [PATCH 29/41] Commander: Be less verbose about param saves to focus the user attention to relevant output --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f274879325..7fbb0a2dac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2794,7 +2794,7 @@ void *commander_low_prio_loop(void *arg) need_param_autosave_timeout = 0; } - mavlink_log_info(mavlink_fd, "settings saved"); + /* do not spam MAVLink, but provide the answer / green led mechanism */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { From fa8dc5723610c5eafddb47526775038cc2a5d77b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 20:00:06 +0200 Subject: [PATCH 30/41] mavlink app: Only send params if the system has booted. --- src/modules/mavlink/mavlink_parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 9c8794017b..524effb205 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -181,8 +181,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) void MavlinkParametersManager::send(const hrt_abstime t) { - /* send all parameters if requested */ - if (_send_all_index >= 0) { + /* send all parameters if requested, but only after the system has booted */ + if (_send_all_index >= 0 && t > 4 * 1000 * 1000) { /* skip if no space is available */ if (_mavlink->get_free_tx_buf() < get_size()) { From 2bcfd4f6f7c03b40ec39083a657a2208fd9a8a65 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 15 May 2015 09:26:07 +0200 Subject: [PATCH 31/41] rename parameters in launch file to match new MP parameters --- launch/ardrone.launch | 16 ++++++++-------- launch/iris.launch | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 56030dd1f4..f43bbf4709 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -6,14 +6,14 @@ - - - - - - - - + + + + + + + + diff --git a/launch/iris.launch b/launch/iris.launch index bf5b099b64..5231e3215b 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -7,14 +7,14 @@ - - - - - - - - + + + + + + + + From 4fb91f47cdc5cb7925c7bf2feb46c64551081883 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 11:15:04 +0200 Subject: [PATCH 32/41] Fix mavlink FTP list stack overflow. Fixes #2180, not flight-critical --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 988232dc21..544d343b43 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1581,7 +1581,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 1800); + pthread_attr_setstacksize(&receiveloop_attr, 2100); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); From f154f6e5e7598b02c2a5c5bb87e646a3425421a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 09:42:35 +0200 Subject: [PATCH 33/41] MAVLink transmission: Allow faster overall transmissions. --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- src/modules/mavlink/mavlink_main.cpp | 12 ++++++------ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index e456eff7f1..8c62abb129 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 30000 -d /dev/ttyACM0 -x +mavlink start -r 80000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 4ccc5dacb6..8b551aa0e8 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -664,7 +664,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=2048 +CONFIG_CDCACM_TXBUFSIZE=4096 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a76abc298d..0df0c3b5bc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 60000 ///< max data rate in bytes/s +#define MAX_DATA_RATE 1000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. @@ -990,7 +990,7 @@ void Mavlink::adjust_stream_rates(const float multiplier) { /* do not allow to push us to zero */ - if (multiplier < 0.01f) { + if (multiplier < 0.0005f) { return; } @@ -1001,9 +1001,9 @@ Mavlink::adjust_stream_rates(const float multiplier) unsigned interval = stream->get_interval(); interval /= multiplier; - /* allow max ~600 Hz */ + /* allow max ~2000 Hz */ if (interval < 1600) { - interval = 1600; + interval = 500; } /* set new interval */ @@ -1375,7 +1375,7 @@ Mavlink::task_main(int argc, char *argv[]) /* MAVLINK_FTP stream */ _mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); - _mavlink_ftp->set_interval(interval_from_rate(120.0f)); + _mavlink_ftp->set_interval(interval_from_rate(1000.0f)); LL_APPEND(_streams, _mavlink_ftp); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on @@ -1425,7 +1425,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; + _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); From 9bd2e376f6bdd4aa31f7c00ba8161fffa293121a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 10:56:39 +0200 Subject: [PATCH 34/41] NuttX for FMUv2: Larger USB buffer --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 8b551aa0e8..a688a1bf9b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -663,8 +663,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=4096 +CONFIG_CDCACM_RXBUFSIZE=1000 +CONFIG_CDCACM_TXBUFSIZE=5000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From dabcf63c506f5ead7978d49df039e4e193cc3a15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 10:28:07 +0200 Subject: [PATCH 35/41] mavlink FTP: Drop rate to realistic value --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0df0c3b5bc..e5cddfafb6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1375,7 +1375,7 @@ Mavlink::task_main(int argc, char *argv[]) /* MAVLINK_FTP stream */ _mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); - _mavlink_ftp->set_interval(interval_from_rate(1000.0f)); + _mavlink_ftp->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _mavlink_ftp); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on From 8f8184560b2a35e64bb27f9386a60a7650d8dec1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 10:16:21 +0200 Subject: [PATCH 36/41] sdlog2: Log at 100 Hz as default --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 454af8da7a..6bfbfea396 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,7 +11,7 @@ then then fi else - if sdlog2 start -r 200 -a -b 22 -t + if sdlog2 start -r 100 -a -b 22 -t then fi fi From 5c53d38652dc6c97e216ea6b70215a95890df572 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 11:38:06 +0200 Subject: [PATCH 37/41] FMUv2 config: Increase USB TX buf size further to speed up MAVLink FTP transfers --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a688a1bf9b..ab8f9eef62 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -664,7 +664,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=1000 -CONFIG_CDCACM_TXBUFSIZE=5000 +CONFIG_CDCACM_TXBUFSIZE=8000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From 97b8e7a20e1bf64b7677911e6bbab300ff471358 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 21:14:34 +0200 Subject: [PATCH 38/41] mavlink FTP: Speed up transfers by longer bursts --- src/modules/mavlink/mavlink_ftp.cpp | 10 ++++++++-- src/modules/mavlink/mavlink_ftp.h | 3 ++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 10b94b6b26..6a87458f72 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -491,6 +491,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id) // Setup for streaming sends _session_info.stream_download = true; _session_info.stream_offset = payload->offset; + _session_info.stream_chunk_transmitted = 0; _session_info.stream_seq_number = payload->seq_number + 1; _session_info.stream_target_system_id = target_system_id; @@ -872,6 +873,7 @@ void MavlinkFTP::send(const hrt_abstime t) } else { payload->size = bytes_read; _session_info.stream_offset += bytes_read; + _session_info.stream_chunk_transmitted += bytes_read; } } @@ -890,8 +892,12 @@ void MavlinkFTP::send(const hrt_abstime t) #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; + /* perform transfers in 35K chunks - this is determined empirical */ + if (_session_info.stream_chunk_transmitted > 35000) { + payload->burst_complete = true; + _session_info.stream_download = false; + _session_info.stream_chunk_transmitted = 0; + } } else { #endif more_data = true; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index af8740e481..33b8996f71 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 @@ -165,6 +165,7 @@ private: uint32_t stream_offset; uint16_t stream_seq_number; uint8_t stream_target_system_id; + unsigned stream_chunk_transmitted; }; struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session From 2432418751b341aae150bccb275a7908ae107ee7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 May 2015 11:37:41 +0200 Subject: [PATCH 39/41] FTP: Better error reporting, ignore hidden directories --- src/modules/mavlink/mavlink_ftp.cpp | 43 ++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a87458f72..6d806415ce 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 @@ -301,23 +301,32 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) MavlinkFTP::ErrorCode MavlinkFTP::_workList(PayloadHeader* payload) { - char dirPath[kMaxDataLength]; - strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); - + char dirPath[kMaxDataLength]; + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); + + ErrorCode errorCode = kErrNone; + unsigned offset = 0; + DIR *dp = opendir(dirPath); if (dp == nullptr) { - warnx("FTP: can't open path '%s'", dirPath); - return kErrFailErrno; + _mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)"); + _mavlink->send_statustext_critical(dirPath); + // this is not an FTP error, abort directory read and continue + + payload->data[offset++] = kDirentSkip; + *((char *)&payload->data[offset]) = '\0'; + offset++; + payload->size = offset; + + return errorCode; } - + #ifdef MAVLINK_FTP_DEBUG warnx("FTP: list %s offset %d", dirPath, payload->offset); #endif - ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; - unsigned offset = 0; // move to the requested offset seekdir(dp, payload->offset); @@ -325,9 +334,16 @@ MavlinkFTP::_workList(PayloadHeader* payload) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrFailErrno; - break; + _mavlink->send_statustext_critical("FTP: list readdir_r failure"); + _mavlink->send_statustext_critical(dirPath); + + payload->data[offset++] = kDirentSkip; + *((char *)&payload->data[offset]) = '\0'; + offset++; + payload->size = offset; + closedir(dp); + + return errorCode; } // no more entries? @@ -357,7 +373,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // XXX @DonLakeFlyer: Remove the first condition for the test setup + if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { From 03ef6a30ec5fa7c09b0de3ba00d577ccca5d39e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 May 2015 11:43:45 +0200 Subject: [PATCH 40/41] Speed up param transmit now that we are faster on USB --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 8c62abb129..0442637941 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ mavlink start -r 80000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB -mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 +mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 From 8a891591a8eee665fe31f5ff4a1d7dfe47ff00a5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 May 2015 19:05:31 +0200 Subject: [PATCH 41/41] make constrain airspeed actually constrain the airspeed --- src/lib/ecl/attitude_fw/ecl_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 9cd08a50d4..95ccf4130f 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -132,9 +132,9 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m float airspeed_result = airspeed; if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (minspeed + maxspeed); + airspeed_result = 0.5f * (minspeed + maxspeed); } else if (airspeed < minspeed) { - airspeed = minspeed; + airspeed_result = minspeed; } return airspeed_result; }