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 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; iflush(); /* 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); 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); 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); 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; 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; 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; 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 { 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; } 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(); 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; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 2ae10bd274..fdb4de4343 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -822,7 +822,8 @@ void VtolAttitudeControl::task_main() /* Only publish if the proper mode(s) are enabled */ if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled) + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);