static int
do_reset_nostart(const char *excludes[], int num_excludes)
{
int32_t autostart;
int32_t autoconfig;
(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
if (num_excludes > 0) {
param_reset_excludes(excludes, num_excludes);
} else {
param_reset_all();
}
(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
int ret = param_save_default();
if (ret) {
PX4_ERR("Param save failed (%i)", ret);
return 1;
}
return 0;
}
static int
do_reset_nostart(const char *excludes[], int num_excludes)
{
int32_t autostart;
int32_t autoconfig;
(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
if (num_excludes > 0) {
param_reset_excludes(excludes, num_excludes);
} else {
param_reset_all();
}
(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
if (param_save_default()) {
warnx("Param export failed.");
return 1;
}
return 0;
}
int do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
}
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs[0],
accel_scale[0],
accel_offs[1],
accel_scale[1],
accel_offs[2],
accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
mavlink_log_info(mavlink_fd, "accel calibration done");
return OK;
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
return ERROR;
}
/* exit accel calibration mode */
}
static int
do_reset(const char *excludes[], int num_excludes)
{
if (num_excludes > 0) {
param_reset_excludes(excludes, num_excludes);
} else {
param_reset_all();
}
if (param_save_default()) {
warnx("Param export failed.");
return 1;
}
return 0;
}
static void
do_reset(const char *excludes[], int num_excludes)
{
if (num_excludes > 0) {
param_reset_excludes(excludes, num_excludes);
} else {
param_reset_all();
}
if (param_save_default()) {
warnx("Param export failed.");
exit(1);
} else {
exit(0);
}
}
static int
do_reset(const char *excludes[], int num_excludes)
{
if (num_excludes > 0) {
param_reset_excludes(excludes, num_excludes);
} else {
param_reset_all();
}
int ret = param_save_default();
if (ret) {
PX4_ERR("Param save failed (%i)", ret);
return 1;
}
return 0;
}
/**
* worker callback method to save the parameters
* @param arg unused
*/
static void
autosave_worker(void *arg)
{
bool disabled = false;
param_lock_writer();
last_autosave_timestamp = hrt_absolute_time();
autosave_scheduled = false;
disabled = autosave_disabled;
param_unlock_writer();
if (disabled) {
return;
}
PX4_DEBUG("Autosaving params");
int ret = param_save_default();
if (ret != 0) {
PX4_ERR("param save failed (%i)", ret);
}
}
int do_trim_calibration(int mavlink_fd)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(200000);
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
if (!changed) {
mavlink_log_critical(mavlink_fd, "no inputs, aborting");
return ERROR;
}
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim cal done");
close(sub_man);
return OK;
}
int do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
/* XXX fix this */
// if (current_status.rc_signal) {
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
// return;
// }
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim calibration done");
close(sub_man);
return OK;
}
int do_accel_calibration(int mavlink_fd)
{
int fd;
int32_t device_id[max_sens];
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,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_sens; s++) {
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
}
float accel_offs[max_sens][3];
float accel_T[max_sens][3][3];
unsigned active_sensors;
if (res == OK) {
/* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
}
if (res != OK || active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
return ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
for (unsigned i = 0; i < active_sensors; i++) {
/* handle individual sensors, one by one */
math::Vector<3> accel_offs_vec(accel_offs[i]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(accel_T[i]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set(param_find(str), &(device_id[i])));
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
return ERROR;
}
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
fd = open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
res = ERROR;
} else {
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
}
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
int do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* erase old calibration */
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
float *x = NULL;
float *y = NULL;
float *z = NULL;
if (res == OK) {
/* allocate memory */
mavlink_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) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mag;
/* limit update rate to get equally spaced measurements over time (in ms) */
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
/* calibrate offsets */
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
calibration_counter = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_mag;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;
z[calibration_counter] = mag.z;
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
}
close(sub_mag);
}
float sphere_x;
float sphere_y;
float sphere_z;
float sphere_radius;
if (res == OK) {
/* sphere fit */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
res = ERROR;
}
}
if (x != NULL)
free(x);
if (y != NULL)
free(y);
if (z != NULL)
free(z);
if (res == OK) {
/* apply calibration and set parameters */
struct mag_scale mscale;
fd = open(MAG_DEVICE_PATH, 0);
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
}
if (res == OK) {
mscale.x_offset = sphere_x;
mscale.y_offset = sphere_y;
mscale.z_offset = sphere_z;
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
close(fd);
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
res = ERROR;
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
res = ERROR;
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
res = ERROR;
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
(double)mscale.y_offset, (double)mscale.z_offset);
mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
}
return res;
}
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
}
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs[0],
accel_scale[0],
accel_offs[1],
accel_scale[1],
accel_offs[2],
accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_error();
sleep(2);
}
/* exit accel calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
}
static int
do_set(const char *name, const char *val, bool fail_on_not_found)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found - fail silenty in scripts as it prevents booting */
PX4_ERR("Parameter %s not found.", name);
return (fail_on_not_found) ? 1 : 0;
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
int32_t newval = strtol(val, &end, 10);
if (i != newval) {
PARAM_PRINT("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
PARAM_PRINT("curr: %ld", (long)i);
param_set_no_autosave(param, &newval);
PARAM_PRINT(" -> new: %ld\n", (long)newval);
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
float newval = strtod(val, &end);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (f != newval) {
#pragma GCC diagnostic pop
PARAM_PRINT("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
PARAM_PRINT("curr: %4.4f", (double)f);
param_set_no_autosave(param, &newval);
PARAM_PRINT(" -> new: %4.4f\n", (double)newval);
}
}
break;
default:
PX4_ERR("<unknown / unsupported type %d>\n", 0 + param_type(param));
return 1;
}
int ret = param_save_default();
if (ret) {
PX4_ERR("Param save failed (%i)", ret);
return 1;
} else {
return 0;
}
}
static int
do_save_default(void)
{
return param_save_default();
}
/* If flash based parameters are uses we have to change some of the calls to the
* default param calls, which will in turn take care of locking and calling to the
* flash backend.
*/
static int
do_save(const char *param_file_name)
{
return param_save_default();
}
int do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
goto error_return;
} else {
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
0.0f,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral(true);
close(diff_pres_sub);
return OK;
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
int
param_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "save")) {
if (argc >= 3) {
do_save(argv[2]);
} else {
if (param_save_default()) {
warnx("Param export failed.");
exit(1);
} else {
exit(0);
}
}
}
if (!strcmp(argv[1], "load")) {
if (argc >= 3) {
do_load(argv[2]);
} else {
do_load(param_get_default_file());
}
}
if (!strcmp(argv[1], "import")) {
if (argc >= 3) {
do_import(argv[2]);
} else {
do_import(param_get_default_file());
}
}
if (!strcmp(argv[1], "select")) {
if (argc >= 3) {
param_set_default_file(argv[2]);
} else {
param_set_default_file(NULL);
}
warnx("selected parameter default file %s", param_get_default_file());
exit(0);
}
if (!strcmp(argv[1], "show")) {
if (argc >= 3) {
do_show(argv[2]);
} else {
do_show(NULL);
}
}
if (!strcmp(argv[1], "set")) {
if (argc >= 4) {
do_set(argv[2], argv[3]);
} else {
errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
}
}
if (!strcmp(argv[1], "compare")) {
if (argc >= 4) {
do_compare(argv[2], &argv[3], argc - 3);
} else {
errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'");
}
}
}
errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
}
bool ParameterTest::exportImportAll()
{
static constexpr float MAGIC_FLOAT_VAL = 0.217828f;
// backup current parameters
const char *param_file_name = PX4_STORAGEDIR "/param_backup";
int fd = open(param_file_name, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
int result = param_export(fd, false);
if (result != PX4_OK) {
PX4_ERR("param_export failed");
close(fd);
return false;
}
close(fd);
bool ret = true;
int N = param_count();
// set all params to corresponding param_t value
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (p == PARAM_INVALID) {
PX4_ERR("param invalid: %d(%d)", p, i);
break;
}
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
param_reset_all();
// restore original params
fd = open(param_file_name, O_RDONLY);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
result = param_import(fd);
close(fd);
if (result < 0) {
PX4_ERR("importing from '%s' failed (%i)", param_file_name, result);
return false;
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
return ret;
}
bool ParameterTest::exportImport()
{
static constexpr float MAGIC_FLOAT_VAL = 0.314159f;
bool ret = true;
param_t test_params[] = {p0, p1, p2, p3, p4};
// set all params to corresponding param_t value
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", set_val, get_val, 0.001f);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL, 0.001f);
}
}
return ret;
}
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
struct gyro_scale gyro_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* determine gyro mean values */
const unsigned calibration_count = 5000;
unsigned calibration_counter = 0;
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
struct gyro_report gyro_report;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_gyro;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
}
close(sub_sensor_gyro);
gyro_scale.x_offset /= calibration_count;
gyro_scale.y_offset /= calibration_count;
gyro_scale.z_offset /= calibration_count;
}
if (res == OK) {
/* check offsets */
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
res = ERROR;
}
}
if (res == OK) {
/* set offset parameters to new values */
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
res = ERROR;
}
}
#if 0
/* beep on offset calibration end */
mavlink_log_info(mavlink_fd, "gyro offset calibration done");
tune_neutral();
/* scale calibration */
/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
warn("WARNING: failed to apply new offsets for gyro");
close(fd);
unsigned rotations_count = 30;
float gyro_integral = 0.0f;
float baseline_integral = 0.0f;
// XXX change to mag topic
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
uint64_t last_time = hrt_absolute_time();
uint64_t start_time = hrt_absolute_time();
while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
close(sub_sensor_combined);
return OK;
}
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
last_time = hrt_absolute_time();
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
// XXX this is just a proof of concept and needs world / body
// transformation and more
//math::Vector2f magNav(raw.magnetometer_ga);
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2 * M_PI_F;
if (mag < -M_PI_F) mag += 2 * M_PI_F;
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2 * M_PI_F;
if (diff < -M_PI_F) diff += 2 * M_PI_F;
baseline_integral += diff;
mag_last = mag;
// Jump through some timing scale hoops to avoid
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
// return;
}
}
float gyro_scale = baseline_integral / gyro_integral;
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
close(sub_sensor_gyro);
mavlink_log_critical(mavlink_fd, "gyro calibration failed");
return ERROR;
}
/* beep on calibration end */
mavlink_log_info(mavlink_fd, "gyro scale calibration done");
tune_neutral();
#endif
if (res == OK) {
/* set scale parameters to new values */
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
res = ERROR;
}
}
if (res == OK) {
/* apply new scaling and offsets */
fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
int do_mag_calibration(int mavlink_fd)
{
const unsigned max_mags = 3;
int32_t device_id[max_mags];
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
sleep(1);
struct mag_scale mscale_null[max_mags] = {
{
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
}
} ;
int res = ERROR;
char str[30];
unsigned calibrated_ok = 0;
for (unsigned s = 0; s < max_mags; s++) {
/* erase old calibration */
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
int fd = open(str, O_RDONLY);
if (fd < 0) {
continue;
}
mavlink_and_console_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s);
sleep(3);
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
/* ensure all scale fields are initialized tha same as the first struct */
(void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
if (res == OK) {
res = calibrate_instance(mavlink_fd, s, device_id[s]);
if (res == OK) {
calibrated_ok++;
}
}
}
if (calibrated_ok) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
usleep(100000);
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
close(diff_pres_sub);
return ERROR;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
}
unsigned calibration_counter = 0;
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(diff_pres_offset)) {
int fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
}
close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
(void)param_save_default();
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral(true);
close(diff_pres_sub);
return OK;
}
int do_mag_calibration(int mavlink_fd)
{
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int result = OK;
// Determine which mags are available and reset each
int32_t device_ids[max_mags];
char str[30];
for (size_t i=0; i<max_mags; i++) {
device_ids[i] = 0; // signals no mag
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
if (fd < 0) {
continue;
}
// Get device id for this mag
device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
// Reset mag scale
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
}
px4_close(fd);
}
// Calibrate all mags at the same time
if (result == OK) {
switch (mag_calibrate_all(mavlink_fd, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
/* give this message enough time to propagate */
usleep(600000);
return result;
}
int do_mag_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
mscale_null.x_scale = 1.0f;
mscale_null.y_offset = 0.0f;
mscale_null.y_scale = 1.0f;
mscale_null.z_offset = 0.0f;
mscale_null.z_scale = 1.0f;
int result = OK;
// Determine which mags are available and reset each
char str[30];
for (size_t i=0; i < max_mags; i++) {
device_ids[i] = 0; // signals no mag
}
_last_mag_progress = 0;
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
#ifndef __PX4_QURT
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.x_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.y_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.z_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.x_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.y_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.z_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
#ifndef __PX4_QURT
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
if (fd < 0) {
continue;
}
// Get device id for this mag
device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0);
// Reset mag scale
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
calibration_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
}
px4_close(fd);
#endif
}
// Calibrate all mags at the same time
if (result == OK) {
switch (mag_calibrate_all(mavlink_log_pub)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
usleep(20000);
break;
} else {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
usleep(20000);
}
// Fall through
default:
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
usleep(20000);
break;
}
}
/* give this message enough time to propagate */
usleep(600000);
return result;
}
int do_accel_calibration(int mavlink_fd)
{
int fd;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* reset all offsets to zero and all scales to one */
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
float accel_offs[3];
float accel_T[3][3];
if (res == OK) {
/* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
}
if (res == OK) {
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
/* set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
res = ERROR;
}
}
if (res == OK) {
/* apply new scaling and offsets */
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
int result = PX4_OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
/* give directions */
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd > 0) {
if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
/* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%llu)", diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again");
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
if (calibration_counter % (calibration_count / 20) == 0) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
// Prevent a completely zero param
// since this is used to detect a missing calibration
// This value is numerically down in the noise and has
// no effect on the sensor performance.
if (fabsf(diff_pres_offset) < 0.00000001f) {
diff_pres_offset = 0.00000001f;
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
break;
} else {
/* do not allow negative values */
calibration_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
param_save_default();
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
* the followup preflight checks might fail. */
usleep(2e6);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = PX4_ERROR;
goto normal_return;
}
int
param_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "save")) {
if (argc >= 3) {
return do_save(argv[2]);
} else {
if (param_save_default()) {
warnx("Param export failed.");
return 1;
} else {
return 0;
}
}
}
if (!strcmp(argv[1], "load")) {
if (argc >= 3) {
return do_load(argv[2]);
} else {
return do_load(param_get_default_file());
}
}
if (!strcmp(argv[1], "import")) {
if (argc >= 3) {
return do_import(argv[2]);
} else {
return do_import(param_get_default_file());
}
}
if (!strcmp(argv[1], "select")) {
if (argc >= 3) {
param_set_default_file(argv[2]);
} else {
param_set_default_file(NULL);
}
warnx("selected parameter default file %s", param_get_default_file());
return 0;
}
if (!strcmp(argv[1], "show")) {
if (argc >= 3) {
do_show(argv[2]);
return 0;
} else {
do_show(NULL);
return 0;
}
}
if (!strcmp(argv[1], "set")) {
if (argc >= 5) {
/* if the fail switch is provided, fails the command if not found */
bool fail = !strcmp(argv[4], "fail");
return do_set(argv[2], argv[3], fail);
} else if (argc >= 4) {
return do_set(argv[2], argv[3], false);
} else {
warnx("not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'");
return 1;
}
}
if (!strcmp(argv[1], "compare")) {
if (argc >= 4) {
return do_compare(argv[2], &argv[3], argc - 3);
} else {
warnx("not enough arguments.\nTry 'param compare PARAM_NAME 3'");
return 1;
}
}
if (!strcmp(argv[1], "reset")) {
if (argc >= 3) {
return do_reset((const char **) &argv[2], argc - 2);
} else {
return do_reset(NULL, 0);
}
}
if (!strcmp(argv[1], "reset_nostart")) {
if (argc >= 3) {
return do_reset_nostart((const char **) &argv[2], argc - 2);
} else {
return do_reset_nostart(NULL, 0);
}
}
if (!strcmp(argv[1], "index_used")) {
if (argc >= 3) {
do_show_index(argv[2], true);
} else {
warnx("no index provided");
return 1;
}
}
if (!strcmp(argv[1], "index")) {
if (argc >= 3) {
do_show_index(argv[2], false);
} else {
warnx("no index provided");
return 1;
}
}
}
warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
return 1;
}
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
const unsigned calibration_count = 5000;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
unsigned calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
unsigned poll_errcount = 0;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
close(sub_sensor_combined);
return ERROR;
}
}
gyro_offset[0] = gyro_offset[0] / calibration_count;
gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count;
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
close(sub_sensor_combined);
return ERROR;
}
tune_neutral();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
close(sub_sensor_combined);
return ERROR;
}
/*** --- SCALING --- ***/
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
unsigned rotations_count = 30;
float gyro_integral = 0.0f;
float baseline_integral = 0.0f;
// XXX change to mag topic
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
uint64_t last_time = hrt_absolute_time();
uint64_t start_time = hrt_absolute_time();
while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
close(sub_sensor_combined);
return OK;
}
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
last_time = hrt_absolute_time();
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
// XXX this is just a proof of concept and needs world / body
// transformation and more
//math::Vector2f magNav(raw.magnetometer_ga);
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2*M_PI_F;
if (mag < -M_PI_F) mag += 2*M_PI_F;
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2*M_PI_F;
if (diff < -M_PI_F) diff += 2*M_PI_F;
baseline_integral += diff;
mag_last = mag;
// Jump through some timing scale hoops to avoid
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
// return;
}
}
float gyro_scale = baseline_integral / gyro_integral;
float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
gyro_scales[0],
gyro_offset[1],
gyro_scales[1],
gyro_offset[2],
gyro_scales[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
// char buf[50];
// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
// mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "gyro calibration done");
/* third beep by cal end routine */
close(sub_sensor_combined);
return OK;
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
close(sub_sensor_combined);
return ERROR;
}
}
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
int fd;
#endif
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set(param_find(str), &accel_scale.x_offset);
if (res != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set(param_find(str), &accel_scale.y_offset);
if (res != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set(param_find(str), &accel_scale.z_offset);
if (res != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set(param_find(str), &accel_scale.x_scale);
if (res != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set(param_find(str), &accel_scale.y_scale);
if (res != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set(param_find(str), &accel_scale.z_scale);
if (res != OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
}
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
unsigned active_sensors;
/* measure and calculate offsets & scales */
if (res == OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return ERROR;
} else if (cal_return == calibrate_return_ok) {
res = OK;
} else {
res = ERROR;
}
}
if (res != OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
for (unsigned i = 0; i < active_sensors; i++) {
/* handle individual sensors, one by one */
math::Vector<3> accel_offs_vec(accel_offs[i]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(accel_T[i]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
/* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
fd = px4_open(str, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
if (res != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i);
}
#endif
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}