fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1919 from PX4/ekf_gyro

Browse Source 
EKF gyro offsetfix
This commit is contained in:
Lorenz Meier 2015-03-16 00:04:03 +01:00
commit bba6f0ae1d
16 changed files with 194 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -15,4 +15,11 @@ then
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
param set PE_VELNE_NOISE 0.3
param set PE_VELD_NOISE 0.5
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 0.5
param set PE_GBIAS_PNOISE 0.000001
param set PE_ABIAS_PNOISE 0.0002
fi

2
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

@ -40,6 +40,8 @@ then
param set PE_VELD_NOISE 0.7
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
param set PE_GBIAS_PNOISE 0.000001
param set PE_ABIAS_PNOISE 0.0001
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0

7
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
View File

@ -32,6 +32,13 @@ then
param set FW_RR_I 0.00
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.02
param set PE_VELNE_NOISE 0.5
param set PE_VELD_NOISE 0.7
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
param set PE_GBIAS_PNOISE 0.000001
param set PE_ABIAS_PNOISE 0.0001
fi
set PWM_DISARMED 900

22
src/drivers/hmc5883/hmc5883.cpp
View File

@ -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)) {

16
src/drivers/l3gd20/l3gd20.cpp
View File

@ -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;

33
src/drivers/lsm303d/lsm303d.cpp
View File

@ -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? */

32
src/drivers/mpu6000/mpu6000.cpp
View File

@ -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;

19
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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]);
}
}

29
src/modules/commander/commander.cpp
View File

@ -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, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
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, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
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) {

32
src/modules/commander/commander_helper.cpp
View File

@ -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)

37
src/modules/commander/gyro_calibration.cpp
View File

@ -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;
}
}

36
src/modules/commander/mag_calibration.cpp
View File

@ -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) {

2
src/modules/commander/module.mk
View File

@ -52,5 +52,5 @@ MODULE_STACKSIZE = 5000
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wframe-larger-than=2000
EXTRACXXFLAGS = -Wframe-larger-than=2200

14
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -784,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) {
@ -1069,7 +1069,7 @@ 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("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
(double)_baro_gps_offset);

4
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
View File

@ -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

26
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -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;
@ -2706,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);
@ -2727,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++) {
@ -2795,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);