diff options
Diffstat (limited to 'src')
20 files changed, 395 insertions, 306 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7560ef20b..2b3520fc8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -848,6 +848,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -907,17 +911,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c41491a8..768723640 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1029,9 +1029,16 @@ L3GD20::measure() report.temperature_raw = raw_report.temp; - report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xraw_f = report.x_raw; + float yraw_f = report.y_raw; + float zraw_f = report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; report.x = _gyro_filter_x.apply(report.x); report.y = _gyro_filter_y.apply(report.y); @@ -1039,9 +1046,6 @@ L3GD20::measure() report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; - // apply user specified rotation - rotate_3f(_rotation, report.x, report.y, report.z); - report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b5f01b942..84f7fb5c8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1517,9 +1517,16 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed @@ -1555,9 +1562,6 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); - accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1623,16 +1627,21 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; - mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // apply user specified rotation - rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); - _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b48ea8577..eaf25a9f3 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1711,17 +1711,21 @@ MPU6000::measure() arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; - float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, arb.x, arb.y, arb.z); - arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1732,17 +1736,21 @@ MPU6000::measure() grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; - float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - // apply user specified rotation - rotate_3f(_rotation, grb.x, grb.y, grb.z); - grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; diff --git a/src/lib/eigen b/src/lib/eigen new file mode 160000 +Subproject e7850ed81f9c469e02df496ef09ae32ec0379b7 diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d70e05000..526b135d8 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -164,11 +164,6 @@ int do_accel_calibration(int mavlink_fd) mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides"); - sleep(3); - mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen"); - sleep(5); - struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -352,7 +347,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ - sleep(3); + sleep(1); int orient = detect_orientation(mavlink_fd, subs); @@ -365,7 +360,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se /* inform user about already handled side */ if (data_collected[orient]) { mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); - sleep(3); + sleep(1); continue; } @@ -374,7 +369,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se read_accelerometer_avg(subs, accel_ref, orient, samples_num); mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); usleep(100000); - mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %8.4f %8.4f %8.4f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], (double)accel_ref[0][orient][2]); @@ -399,13 +394,6 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se for (unsigned i = 0; i < (*active_sensors); i++) { res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* verbose output on the console */ - printf("accel_T transformation matrix:\n"); - for (unsigned j = 0; j < 3; j++) { - printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); - } - printf("\n"); - if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; @@ -635,7 +623,6 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6 for (unsigned s = 0; s < max_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; - warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]); } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4e119a110..b8638c6a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -278,7 +278,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3200, + 3400, commander_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); @@ -967,19 +967,6 @@ int commander_thread_main(int argc, char *argv[]) int ret; - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2000); - - struct sched_param param; - (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - pthread_attr_destroy(&commander_low_prio_attr); - /* Start monitoring loop */ unsigned counter = 0; unsigned stick_off_counter = 0; @@ -1144,6 +1131,20 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = false; bool failsafe_old = false; + /* initialize low priority thread */ + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2100); + + struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index b5ec6c4e6..a2e827a15 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -231,7 +231,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED0_DEVICE_PATH, 0); - if (rgbleds == -1) { + if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); } @@ -240,50 +240,64 @@ int led_init() void led_deinit() { - close(leds); + if (leds >= 0) { + close(leds); + } - if (rgbleds != -1) { + if (rgbleds >= 0) { close(rgbleds); } } int led_toggle(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_ON, led); } int led_off(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index dfd1657c5..8a70cf66e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -73,16 +73,17 @@ int do_gyro_calibration(int mavlink_fd) /* wait for the user to respond */ sleep(2); - struct gyro_scale gyro_scale[max_gyros] = { { + struct gyro_scale gyro_scale_zero = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } }; + struct gyro_scale gyro_scale[max_gyros]; + int res = OK; /* store board ID */ @@ -97,7 +98,7 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0])); + (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ @@ -109,7 +110,7 @@ int do_gyro_calibration(int mavlink_fd) device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); close(fd); if (res != OK) { @@ -120,6 +121,8 @@ int do_gyro_calibration(int mavlink_fd) unsigned calibration_counter[max_gyros] = { 0 }; const unsigned calibration_count = 5000; + struct gyro_report gyro_report_0 = {}; + if (res == OK) { /* determine gyro mean values */ unsigned poll_errcount = 0; @@ -140,7 +143,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); + int poll_ret = poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -150,6 +153,11 @@ int do_gyro_calibration(int mavlink_fd) if (changed) { orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); + } + gyro_scale[s].x_offset += gyro_report.x; gyro_scale[s].y_offset += gyro_report.y; gyro_scale[s].z_offset += gyro_report.z; @@ -183,8 +191,20 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* check offsets */ - if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) { - mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; + float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.01f; + + if (!isfinite(gyro_scale[0].x_offset) || + !isfinite(gyro_scale[0].y_offset) || + !isfinite(gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed"); res = ERROR; } } @@ -214,6 +234,7 @@ int do_gyro_calibration(int mavlink_fd) int fd = open(str, 0); if (fd < 0) { + failed = true; continue; } @@ -226,7 +247,7 @@ int do_gyro_calibration(int mavlink_fd) } if (failed) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e0786db79..a0aadab39 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -164,9 +164,9 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; - float *x = NULL; - float *y = NULL; - float *z = NULL; + float *x = new float[calibration_maxcount]; + float *y = new float[calibration_maxcount]; + float *z = new float[calibration_maxcount]; char str[30]; int res = OK; @@ -174,24 +174,20 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) /* allocate memory */ mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); - y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); - z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); - - if (x == NULL || y == NULL || z == NULL) { + if (x == nullptr || y == nullptr || z == nullptr) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); /* clean up */ - if (x != NULL) { - free(x); + if (x != nullptr) { + delete x; } - if (y != NULL) { - free(y); + if (y != nullptr) { + delete y; } - if (z != NULL) { - free(z); + if (z != nullptr) { + delete z; } res = ERROR; @@ -274,16 +270,16 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) } } - if (x != NULL) { - free(x); + if (x != nullptr) { + delete x; } - if (y != NULL) { - free(y); + if (y != nullptr) { + delete y; } - if (z != NULL) { - free(z); + if (z != nullptr) { + delete z; } if (res == OK) { diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4a1369aba..4ee8732fc 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,5 +52,5 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=2000 +EXTRACXXFLAGS = -Wframe-larger-than=2200 diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ec9efe8ee..4d5e56a96 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,6 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -194,6 +193,7 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; + float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; @@ -208,7 +208,6 @@ private: bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 - bool _newDataGps; bool _newHgtData; bool _newAdsData; bool _newDataMag; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3bb395a87..c16e72ea0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -132,72 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _wind_pub(-1), _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, - - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), - - _sensor_combined {}, - - _pos_ref {}, - _baro_ref(0.0f), - _baro_ref_offset(0.0f), - _baro_gps_offset(0.0f), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), - - _newDataGps(false), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), - - _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + _wind({}), + _distance {}, + _landDetector {}, + _armed {}, + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + _sensor_combined {}, + + _pos_ref{}, + _baro_ref_offset(0.0f), + _baro_gps_offset(0.0f), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _baroAltRef(0.0f), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), + _distance_last_valid(0), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), + _gyro_main(0), + _accel_main(0), + _mag_main(0), + _ekf_logging(true), + _debug(0), + + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + + _mavlink_fd(-1), + _parameters {}, + _parameter_handles {}, + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -636,24 +635,26 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gpsIsGood) { - initializeGPS(); - - } else if (!_ekf->statesInitialised) { + if (!_ekf->statesInitialised) { // North, East Down position (m) float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; - _local_pos.ref_alt = _baro_ref; + _local_pos.ref_alt = 0.0f; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } else if (_ekf->statesInitialised) { + } else { + + if (!_gps_initialized && _gpsIsGood) { + initializeGPS(); + continue; + } // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); @@ -668,7 +669,7 @@ void AttitudePositionEstimatorEKF::task_main() } //Run EKF data fusion steps - updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); @@ -717,7 +718,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + _ekf->hgtMea = _ekf->baroHgt; // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; @@ -783,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets - _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU; - _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU; - _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU; + _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; + _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt; + _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; + _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -858,7 +859,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -908,8 +909,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() } void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, - const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed) + const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed) { // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); @@ -978,7 +978,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseBaro) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->hgtMea = _ekf->baroHgt; _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays @@ -1069,9 +1069,9 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, + printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1268,10 +1268,10 @@ void AttitudePositionEstimatorEKF::pollData() } - orb_check(_gps_sub, &_newDataGps); - - if (_newDataGps) { + bool gps_update; + orb_check(_gps_sub, &gps_update); + if (gps_update) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -1324,10 +1324,7 @@ void AttitudePositionEstimatorEKF::pollData() if (_gps_initialized) { //Convert from global frame to local frame - float posNED[3] = {0.0f, 0.0f, 0.0f}; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]); if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); @@ -1353,9 +1350,6 @@ void AttitudePositionEstimatorEKF::pollData() _previousGPSTimestamp = _gps.timestamp_position; - } else { - //Too poor GPS fix to use - _newDataGps = false; } } @@ -1406,21 +1400,17 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; + if(!_baro_init) { + _baro_init = true; + _baroAltRef = _baro.altitude; + } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } - perf_count(_perf_baro); - - _newHgtData = true; } //Update Magnetometer diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 8c82dd6c1..f8cca6c0d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -218,7 +218,7 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); /** * Accelerometer bias estimate process noise @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); /** * Magnetometer earth frame offsets process noise diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5c01286e3..817590bab 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -210,10 +210,10 @@ void AttPosEKF::InitialiseParameters() yawVarScale = 1.0f; windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; + dAngBiasSigma = 1.0e-6; + dVelBiasSigma = 0.0002f; + magEarthSigma = 0.0003f; + magBodySigma = 0.0003f; vneSigma = 0.2f; vdSigma = 0.3f; @@ -414,9 +414,10 @@ void AttPosEKF::CovariancePrediction(float dt) // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + float gyroBiasScale = (_onGround) ? 2.0f : 1.0f; + + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale; processNoise[13] = dVelBiasSigma; if (!inhibitWindStates) { for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; @@ -2329,15 +2330,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (size_t i=0; i<EKF_STATE_ESTIMATES; i++) + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { storedStates[i][storeIndex] = states[i]; + } + storedOmega[0][storeIndex] = angRate.x; storedOmega[1][storeIndex] = angRate.y; storedOmega[2][storeIndex] = angRate.z; statetimeStamp[storeIndex] = timestamp_ms; + + // increment to next storage index storeIndex++; - if (storeIndex == EKF_DATA_BUFFER_SIZE) + if (storeIndex >= EKF_DATA_BUFFER_SIZE) { storeIndex = 0; + } } void AttPosEKF::ResetStoredStates() @@ -2350,10 +2356,8 @@ void AttPosEKF::ResetStoredStates() // reset store index to first storeIndex = 0; - statetimeStamp[storeIndex] = millis(); - - // increment to next storage index - storeIndex++; + //Reset stored state to current state + StoreStates(millis()); } // Output the state vector stored at the time that best matches that specified by msec @@ -2513,27 +2517,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) -{ - posNED[0] = earthRadius * (lat - latReference); - posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); - posNED[2] = -(hgt - hgtReference); -} - -void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) -{ - lat = latRef + (double)posNED[0] * earthRadiusInv; - lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - void AttPosEKF::setOnGround(const bool isLanded) { _onGround = isLanded; @@ -2592,25 +2575,40 @@ void AttPosEKF::CovarianceInit() P[1][1] = 0.25f * sq(1.0f*deg2rad); P[2][2] = 0.25f * sq(1.0f*deg2rad); P[3][3] = 0.25f * sq(10.0f*deg2rad); + + //velocities P[4][4] = sq(0.7f); P[5][5] = P[4][4]; P[6][6] = sq(0.7f); + + //positions P[7][7] = sq(15.0f); P[8][8] = P[7][7]; P[9][9] = sq(5.0f); + + //delta angle biases P[10][10] = sq(0.1f*deg2rad*dtIMU); P[11][11] = P[10][10]; P[12][12] = P[10][10]; + + //Z delta velocity bias P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(0.0f); + + //Wind velocities + P[14][14] = 0.0f; P[15][15] = P[14][14]; + + //Earth magnetic field P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; + + //Body magnetic field P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; + //Optical flow fScaleFactorVar = 0.001f; // focal length scale factor variance Popt[0][0] = 0.001f; } @@ -2628,9 +2626,11 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } +#if 0 if (!isfinite(val)) { - //ekf_debug("constrain: non-finite!"); + ekf_debug("constrain: non-finite!"); } +#endif return ret; } @@ -2707,6 +2707,11 @@ void AttPosEKF::ConstrainStates() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // Constrain dtIMUfilt + if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { + dtIMUfilt = dtIMU; + } + // Constrain quaternion for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); @@ -2728,11 +2733,11 @@ void AttPosEKF::ConstrainStates() // Angle bias limit - set to 8 degrees / sec for (size_t i = 10; i <= 12; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt); } // Constrain delta velocity bias - states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt); // Wind velocity limits - assume 120 m/s max velocity for (size_t i = 14; i <= 15; i++) { @@ -2796,8 +2801,8 @@ bool AttPosEKF::GyroOffsetsDiverged() // Protect against division by zero if (delta_len > 0.0f) { - float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); - delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f); + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt; } bool diverged = (delta_len_scaled > 1.0f); @@ -2863,8 +2868,12 @@ void AttPosEKF::ResetPosition(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[7][i] = states[7]; storedStates[8][i] = states[8]; - } + } } + + //reset position covariance + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; } void AttPosEKF::ResetHeight(void) @@ -2876,6 +2885,10 @@ void AttPosEKF::ResetHeight(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[9][i] = states[9]; } + + //reset altitude covariance + P[9][9] = sq(5.0f); + P[6][6] = sq(0.7f); } void AttPosEKF::ResetVelocity(void) @@ -2884,7 +2897,8 @@ void AttPosEKF::ResetVelocity(void) states[4] = 0.0f; states[5] = 0.0f; states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { + } + else if (GPSstatus >= GPS_FIX_3D) { //Do not use Z velocity, we trust the Barometer history more states[4] = velNED[0]; // north velocity from last reading @@ -2894,8 +2908,12 @@ void AttPosEKF::ResetVelocity(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[4][i] = states[4]; storedStates[5][i] = states[5]; - } + } } + + //reset velocities covariance + P[4][4] = sq(0.7f); + P[5][5] = P[4][4]; } bool AttPosEKF::StatesNaN() { @@ -3012,10 +3030,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // Timeout cleared with this reset current_ekf_state.imuTimeout = false; @@ -3029,10 +3047,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report, but not setting error flag GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); ret = 0; } @@ -3202,10 +3220,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // initialise focal length scale factor estimator states flowStates[0] = 1.0f; @@ -3217,7 +3235,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -3293,7 +3310,6 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); - } void AttPosEKF::GetFilterState(struct ekf_status_report *err) @@ -3310,13 +3326,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); - - // err->velHealth = current_ekf_state.velHealth; - // err->posHealth = current_ekf_state.posHealth; - // err->hgtHealth = current_ekf_state.hgtHealth; - // err->velTimeout = current_ekf_state.velTimeout; - // err->posTimeout = current_ekf_state.posTimeout; - // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index c5517e38b..9b23f4df4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -288,7 +288,6 @@ public: * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -307,12 +306,6 @@ public: static void quat2eul(float (&eul)[3], const float (&quat)[4]); - static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - - static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); - - static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); - //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static inline float sq(float valIn) {return valIn * valIn;} @@ -362,8 +355,6 @@ public: */ void ResetVelocity(); - void ZeroVariables(); - void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); @@ -381,6 +372,12 @@ public: * true if the vehicle moves like a Fixed Wing, false otherwise **/ void setIsFixedWing(const bool fixedWing); + + /** + * @brief + * Reset internal filter states and clear all variables to zero value + */ + void ZeroVariables(); protected: @@ -409,7 +406,7 @@ protected: void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); void ResetStoredStates(); - + private: bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 401cf05f1..85a0dda9e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -166,14 +166,17 @@ private: param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_d; + param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_d; + param_t pitch_rate_ff; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_rate_ff; param_t yaw_ff; param_t yaw_rate_max; @@ -191,6 +194,7 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ @@ -316,6 +320,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; @@ -335,14 +340,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); @@ -394,6 +402,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_rate_ff, &v); + _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); @@ -404,6 +414,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_rate_ff, &v); + _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -414,6 +426,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_rate_ff, &v); + _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); @@ -653,7 +667,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; /* update integral only if not saturated on low limit */ diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 302551959..6a21f9046 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -83,6 +83,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); /** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + +/** * Pitch P gain * * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -124,6 +134,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); /** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + +/** * Yaw P gain * * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -165,6 +185,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); /** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + +/** * Yaw feed forward * * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 5459fcffd..2758979a2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -57,7 +57,8 @@ Mavlink::Mavlink() : { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); - + _att_sp = {}; + _offboard_control_mode = {}; } int main(int argc, char **argv) @@ -127,10 +128,8 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); - static offboard_control_mode offboard_control_mode; - /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * if attitude or body rate have been used (not ignored) previously and this message only sends @@ -140,45 +139,46 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate && ignore_attitude && !_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 && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _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; + _offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; - - offboard_control_mode.timestamp = get_time_micros(); - _offboard_control_mode_pub.publish(offboard_control_mode); + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - static vehicle_attitude_setpoint att_sp = {}; + _offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(_offboard_control_mode); /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint * gets published only if in offboard mode. We leave that out for now. */ - att_sp.timestamp = get_time_micros(); - 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.data()); - att_sp.R_valid = true; + _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { + 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.data()); + _att_sp.R_valid = true; + } + - if (!offboard_control_mode.ignore_thrust) { - att_sp.thrust = set_attitude_target.thrust; + if (!_offboard_control_mode.ignore_thrust) { + _att_sp.thrust = set_attitude_target.thrust; } - if (!offboard_control_mode.ignore_attitude) { + if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + _att_sp.q_d[i] = set_attitude_target.q[i]; } - att_sp.q_d_valid = true; + _att_sp.q_d_valid = true; } - _v_att_sp_pub.publish(att_sp); + _v_att_sp_pub.publish(_att_sp); //XXX real mavlink publishes rate sp here diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index acb2408f3..8b7d08d24 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -69,6 +69,8 @@ protected: ros::Publisher _pos_sp_triplet_pub; ros::Publisher _offboard_control_mode_pub; ros::Publisher _force_sp_pub; + vehicle_attitude_setpoint _att_sp; + offboard_control_mode _offboard_control_mode; /** * |