forked from Archive/PX4-Autopilot
Merge pull request #33 from julianoes/master
auto save after calibration (however the rest is reset to stock)
This commit is contained in:
commit
a8b2e40b31
|
@ -136,6 +136,7 @@ static void led_deinit(void);
|
|||
static int led_toggle(int led);
|
||||
static int led_on(int led);
|
||||
static int led_off(int led);
|
||||
static int pm_save_eeprom(bool only_unsaved);
|
||||
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
|
@ -266,6 +267,33 @@ static void cal_bsort(float a[], int n)
|
|||
}
|
||||
}
|
||||
|
||||
static const char *parameter_file = "/eeprom/parameters";
|
||||
|
||||
static int pm_save_eeprom(bool only_unsaved)
|
||||
{
|
||||
/* delete the file in case it exists */
|
||||
unlink(parameter_file);
|
||||
|
||||
/* create the file */
|
||||
int fd = open(parameter_file, O_WRONLY | O_CREAT | O_EXCL);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("opening '%s' for writing failed", parameter_file);
|
||||
return -1;
|
||||
}
|
||||
|
||||
int result = param_export(fd, only_unsaved);
|
||||
close(fd);
|
||||
|
||||
if (result != 0) {
|
||||
unlink(parameter_file);
|
||||
warn("error exporting parameters to '%s'", parameter_file);
|
||||
return -2;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
{
|
||||
/* set to mag calibration mode */
|
||||
|
@ -487,6 +515,14 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
free(mag_minima[1]);
|
||||
free(mag_minima[2]);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = pm_save_eeprom(false);
|
||||
if(save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to EEPROM failed");
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished");
|
||||
|
||||
close(sub_sensor_combined);
|
||||
}
|
||||
|
||||
|
@ -566,10 +602,18 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
warn("WARNING: failed to set scale / offsets for gyro");
|
||||
close(fd);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = pm_save_eeprom(false);
|
||||
if(save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to EEPROM failed");
|
||||
}
|
||||
|
||||
/* exit to gyro calibration mode */
|
||||
status->flag_preflight_gyro_calibration = false;
|
||||
state_machine_publish(status_pub, status, mavlink_fd);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished");
|
||||
|
||||
// char offset_output[50];
|
||||
// sprintf(offset_output, "[commander] gyro 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, offset_output);
|
||||
|
@ -675,10 +719,18 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
warn("WARNING: failed to set scale / offsets for accel");
|
||||
close(fd);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = pm_save_eeprom(false);
|
||||
if(save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to EEPROM failed");
|
||||
}
|
||||
|
||||
/* exit to gyro calibration mode */
|
||||
status->flag_preflight_accel_calibration = false;
|
||||
state_machine_publish(status_pub, status, mavlink_fd);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished");
|
||||
|
||||
// char offset_output[50];
|
||||
// sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
|
||||
// (double)accel_offset[1], (double)accel_offset[2]);
|
||||
|
|
Loading…
Reference in New Issue