mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 23:03:35 +08:00
Minor fixes to HMC driver, mag calibration done
This commit is contained in:
Lorenz Meier
+191
-11
@@ -6,7 +6,6 @@
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
@@ -104,6 +103,8 @@ static int stat_pub;
|
||||
static uint16_t nofix_counter = 0;
|
||||
static uint16_t gotfix_counter = 0;
|
||||
|
||||
static void do_gyro_calibration(void);
|
||||
static void do_mag_calibration(void);
|
||||
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
|
||||
|
||||
/* pthread loops */
|
||||
@@ -212,16 +213,175 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u
|
||||
return 0;
|
||||
}
|
||||
|
||||
void cal_bsort(int16_t a[], int n)
|
||||
{
|
||||
int i,j,t;
|
||||
for(i=0;i<n-1;i++)
|
||||
{
|
||||
for(j=0;j<n-i-1;j++)
|
||||
{
|
||||
if(a[j]>a[j+1]) {
|
||||
t=a[j];
|
||||
a[j]=a[j+1];
|
||||
a[j+1]=t;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void do_mag_calibration(void)
|
||||
{
|
||||
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
/* 30 seconds */
|
||||
const uint64_t calibration_interval_us = 5 * 1000000;
|
||||
unsigned int calibration_counter = 0;
|
||||
|
||||
const int peak_samples = 2000;
|
||||
/* Get rid of 10% */
|
||||
const int outlier_margin = (peak_samples) / 10;
|
||||
|
||||
int16_t *mag_maxima[3];
|
||||
mag_maxima[0] = (int16_t*)calloc(peak_samples, sizeof(uint16_t));
|
||||
mag_maxima[1] = (int16_t*)calloc(peak_samples, sizeof(uint16_t));
|
||||
mag_maxima[2] = (int16_t*)calloc(peak_samples, sizeof(uint16_t));
|
||||
|
||||
int16_t *mag_minima[3];
|
||||
mag_minima[0] = (int16_t*)calloc(peak_samples, sizeof(uint16_t));
|
||||
mag_minima[1] = (int16_t*)calloc(peak_samples, sizeof(uint16_t));
|
||||
mag_minima[2] = (int16_t*)calloc(peak_samples, sizeof(uint16_t));
|
||||
|
||||
/* initialize data table */
|
||||
for (int i = 0; i < peak_samples; i++) {
|
||||
mag_maxima[0][i] = INT16_MIN;
|
||||
mag_maxima[1][i] = INT16_MIN;
|
||||
mag_maxima[2][i] = INT16_MIN;
|
||||
|
||||
mag_minima[0][i] = INT16_MAX;
|
||||
mag_minima[1][i] = INT16_MAX;
|
||||
mag_minima[2][i] = INT16_MAX;
|
||||
}
|
||||
|
||||
uint64_t calibration_start = hrt_absolute_time();
|
||||
while ((hrt_absolute_time() - calibration_start) < calibration_interval_us
|
||||
&& calibration_counter < peak_samples) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
|
||||
|
||||
if (poll(fds, 1, 1000)) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
/* get min/max values */
|
||||
|
||||
/* iterate through full list */
|
||||
for (int i = 0; i < peak_samples; i++) {
|
||||
/* x minimum */
|
||||
if (raw.magnetometer_raw[0] < mag_minima[0][i])
|
||||
mag_minima[0][i] = raw.magnetometer_raw[0];
|
||||
/* y minimum */
|
||||
if (raw.magnetometer_raw[1] < mag_minima[1][i])
|
||||
mag_minima[1][i] = raw.magnetometer_raw[1];
|
||||
/* z minimum */
|
||||
if (raw.magnetometer_raw[2] < mag_minima[2][i])
|
||||
mag_minima[2][i] = raw.magnetometer_raw[2];
|
||||
|
||||
/* x maximum */
|
||||
if (raw.magnetometer_raw[0] > mag_maxima[0][i])
|
||||
mag_maxima[0][i] = raw.magnetometer_raw[0];
|
||||
/* y maximum */
|
||||
if (raw.magnetometer_raw[1] > mag_maxima[1][i])
|
||||
mag_maxima[1][i] = raw.magnetometer_raw[1];
|
||||
/* z maximum */
|
||||
if (raw.magnetometer_raw[2] > mag_maxima[2][i])
|
||||
mag_maxima[2][i] = raw.magnetometer_raw[2];
|
||||
}
|
||||
|
||||
calibration_counter++;
|
||||
} else {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* sort values */
|
||||
cal_bsort(mag_minima[0], peak_samples);
|
||||
cal_bsort(mag_minima[1], peak_samples);
|
||||
cal_bsort(mag_minima[2], peak_samples);
|
||||
|
||||
cal_bsort(mag_maxima[0], peak_samples);
|
||||
cal_bsort(mag_maxima[1], peak_samples);
|
||||
cal_bsort(mag_maxima[2], peak_samples);
|
||||
|
||||
float min_avg[3] = { 0.0f, 0.0f, 0.0f };
|
||||
float max_avg[3] = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
/* take average of center value group */
|
||||
for (int i = 0; i < (peak_samples - outlier_margin); i++) {
|
||||
min_avg[0] += mag_minima[0][i+outlier_margin];
|
||||
min_avg[1] += mag_minima[1][i+outlier_margin];
|
||||
min_avg[2] += mag_minima[2][i+outlier_margin];
|
||||
|
||||
max_avg[0] += mag_maxima[0][i];
|
||||
max_avg[1] += mag_maxima[1][i];
|
||||
max_avg[2] += mag_maxima[2][i];
|
||||
|
||||
if (i > (peak_samples - outlier_margin)-15) {
|
||||
printf("mag min: %d\t%d\t%d\tmag max: %d\t%d\t%d\n",
|
||||
mag_minima[0][i+outlier_margin],
|
||||
mag_minima[1][i+outlier_margin],
|
||||
mag_minima[2][i+outlier_margin],
|
||||
mag_maxima[0][i],
|
||||
mag_maxima[1][i],
|
||||
mag_maxima[2][i]);
|
||||
usleep(10000);
|
||||
}
|
||||
}
|
||||
|
||||
min_avg[0] /= (peak_samples - outlier_margin);
|
||||
min_avg[1] /= (peak_samples - outlier_margin);
|
||||
min_avg[2] /= (peak_samples - outlier_margin);
|
||||
|
||||
max_avg[0] /= (peak_samples - outlier_margin);
|
||||
max_avg[1] /= (peak_samples - outlier_margin);
|
||||
max_avg[2] /= (peak_samples - outlier_margin);
|
||||
|
||||
printf("\nFINAL:\nmag min: %d\t%d\t%d\nmag max: %d\t%d\t%d\n", (int)min_avg[0], (int)min_avg[1], (int)min_avg[2], (int)max_avg[0], (int)max_avg[1], (int)max_avg[2]);
|
||||
|
||||
int16_t mag_offset[3];
|
||||
mag_offset[0] = (max_avg[0] - min_avg[0])/2;
|
||||
mag_offset[1] = (max_avg[1] - min_avg[1])/2;
|
||||
mag_offset[2] = (max_avg[2] - min_avg[2])/2;
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET] = mag_offset[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET] = mag_offset[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET] = mag_offset[2];
|
||||
|
||||
free(mag_maxima[0]);
|
||||
free(mag_maxima[1]);
|
||||
free(mag_maxima[2]);
|
||||
|
||||
free(mag_minima[0]);
|
||||
free(mag_minima[1]);
|
||||
free(mag_minima[2]);
|
||||
|
||||
char offset_output[50];
|
||||
sprintf(offset_output, "[commander] mag calibration finished, offsets: x:%d, y:%d, z:%d", mag_offset[0], mag_offset[1], mag_offset[2]);
|
||||
mavlink_log_info(mavlink_fd, offset_output);
|
||||
|
||||
close(sub_sensor_combined);
|
||||
}
|
||||
|
||||
void do_gyro_calibration(void)
|
||||
{
|
||||
|
||||
const int calibration_count = 3000;
|
||||
|
||||
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
int calibration_counter = 0;
|
||||
float gyro_offset[3] = {0, 0, 0};
|
||||
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
@@ -234,6 +394,10 @@ void do_gyro_calibration(void)
|
||||
gyro_offset[1] += raw.gyro_raw[1];
|
||||
gyro_offset[2] += raw.gyro_raw[2];
|
||||
calibration_counter++;
|
||||
} else {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry.");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -325,12 +489,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||
//
|
||||
/* preflight calibration */
|
||||
case MAV_CMD_PREFLIGHT_CALIBRATION: {
|
||||
if (cmd->param1 == 1.0) {
|
||||
bool handled = false;
|
||||
|
||||
/* gyro calibration */
|
||||
if ((int)(cmd->param1) == 1) {
|
||||
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
|
||||
do_gyro_calibration();
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
handled = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* magnetometer calibration */
|
||||
if ((int)(cmd->param2) == 1) {
|
||||
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
|
||||
do_mag_calibration();
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
|
||||
/* none found */
|
||||
if (!handled) {
|
||||
fprintf(stderr, "[commander] refusing unsupported calibration request\n");
|
||||
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request");
|
||||
result = MAV_RESULT_UNSUPPORTED;
|
||||
@@ -342,7 +519,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||
case MAV_CMD_PREFLIGHT_STORAGE: {
|
||||
/* Read all parameters from EEPROM to RAM */
|
||||
|
||||
if (cmd->param1 == 0.0) {
|
||||
if (((int)cmd->param1) == 0) {
|
||||
|
||||
if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
|
||||
printf("[commander] Loaded EEPROM params in RAM\n");
|
||||
@@ -357,7 +534,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||
|
||||
/* Write all parameters from RAM to EEPROM */
|
||||
|
||||
} else if (cmd->param1 == 1.0) {
|
||||
} else if (((int)cmd->param1) == 1) {
|
||||
|
||||
if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
|
||||
printf("[commander] RAM params written to EEPROM\n");
|
||||
@@ -574,7 +751,7 @@ int commander_main(int argc, char *argv[])
|
||||
/* create pthreads */
|
||||
pthread_attr_t command_handling_attr;
|
||||
pthread_attr_init(&command_handling_attr);
|
||||
pthread_attr_setstacksize(&command_handling_attr, 3072);
|
||||
pthread_attr_setstacksize(&command_handling_attr, 4096);
|
||||
pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL);
|
||||
|
||||
// pthread_attr_t subsystem_info_attr;
|
||||
@@ -608,13 +785,16 @@ int commander_main(int argc, char *argv[])
|
||||
|
||||
/* Subscribe to RC data */
|
||||
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
struct rc_channels_s rc = {0};
|
||||
struct rc_channels_s rc;
|
||||
memset(&rc, 0, sizeof(rc));
|
||||
|
||||
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
struct vehicle_gps_position_s gps = {0};
|
||||
struct vehicle_gps_position_s gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
|
||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
struct sensor_combined_s sensors = {0};
|
||||
struct sensor_combined_s sensors;
|
||||
memset(&sensors, 0, sizeof(sensors));
|
||||
|
||||
uint8_t vehicle_state_previous = current_status.state_machine;
|
||||
|
||||
|
||||
+23
-11
@@ -300,17 +300,17 @@ int sensors_main(int argc, char *argv[])
|
||||
unsigned int adc_fail_count = 0;
|
||||
unsigned int adc_success_count = 0;
|
||||
|
||||
ssize_t ret_gyro;
|
||||
ssize_t ret_accelerometer;
|
||||
ssize_t ret_magnetometer;
|
||||
ssize_t ret_barometer;
|
||||
ssize_t ret_adc;
|
||||
int nsamples_adc;
|
||||
ssize_t ret_gyro;
|
||||
ssize_t ret_accelerometer;
|
||||
ssize_t ret_magnetometer;
|
||||
ssize_t ret_barometer;
|
||||
ssize_t ret_adc;
|
||||
int nsamples_adc;
|
||||
|
||||
int16_t buf_gyro[3];
|
||||
int16_t buf_accelerometer[3];
|
||||
int16_t buf_magnetometer[3];
|
||||
float buf_barometer[3];
|
||||
int16_t buf_magnetometer[7];
|
||||
float buf_barometer[3];
|
||||
|
||||
int16_t mag_offset[3] = {0, 0, 0};
|
||||
int16_t acc_offset[3] = {0, 0, 0};
|
||||
@@ -335,7 +335,7 @@ int sensors_main(int argc, char *argv[])
|
||||
float battery_voltage_conversion;
|
||||
battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION];
|
||||
|
||||
if (-1.0f == battery_voltage_conversion) {
|
||||
if (-1 == (int)battery_voltage_conversion) {
|
||||
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
|
||||
battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f;
|
||||
}
|
||||
@@ -387,11 +387,13 @@ int sensors_main(int argc, char *argv[])
|
||||
publishing = true;
|
||||
|
||||
/* advertise the rc topic */
|
||||
struct rc_channels_s rc = {0};
|
||||
struct rc_channels_s rc;
|
||||
memset(&rc, 0, sizeof(rc));
|
||||
int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
|
||||
|
||||
/* subscribe to system status */
|
||||
struct vehicle_status_s vstatus = {0};
|
||||
struct vehicle_status_s vstatus;
|
||||
memset(&vstatus, 0, sizeof(vstatus));
|
||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
|
||||
@@ -562,6 +564,13 @@ int sensors_main(int argc, char *argv[])
|
||||
/* MAGNETOMETER */
|
||||
if (magcounter == 4) { /* 120 Hz */
|
||||
uint64_t start_mag = hrt_absolute_time();
|
||||
/* start calibration mode if requested */
|
||||
if (raw.magnetometer_mode == MAGNETOMETER_MODE_NORMAL && vstatus.preflight_mag_calibration) {
|
||||
ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0);
|
||||
} else if (raw.magnetometer_mode != MAGNETOMETER_MODE_NORMAL && !vstatus.preflight_mag_calibration) {
|
||||
ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0);
|
||||
}
|
||||
|
||||
ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
|
||||
int errcode_mag = (int) * get_errno_ptr();
|
||||
int magtime = hrt_absolute_time() - start_mag;
|
||||
@@ -765,6 +774,9 @@ int sensors_main(int argc, char *argv[])
|
||||
raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f;
|
||||
raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f;
|
||||
|
||||
/* store mode */
|
||||
raw.magnetometer_mode = buf_magnetometer[3];
|
||||
|
||||
raw.magnetometer_raw_counter++;
|
||||
}
|
||||
|
||||
|
||||
@@ -51,6 +51,12 @@
|
||||
* @{
|
||||
*/
|
||||
|
||||
enum MAGNETOMETER_MODE {
|
||||
MAGNETOMETER_MODE_NORMAL = 0,
|
||||
MAGNETOMETER_MODE_POSITIVE_BIAS,
|
||||
MAGNETOMETER_MODE_NEGATIVE_BIAS
|
||||
};
|
||||
|
||||
/**
|
||||
* Sensor readings in raw and SI-unit form.
|
||||
*
|
||||
@@ -71,25 +77,33 @@ struct sensor_combined_s {
|
||||
|
||||
/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */
|
||||
|
||||
uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */
|
||||
uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */
|
||||
|
||||
int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */
|
||||
uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */
|
||||
float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */
|
||||
int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */
|
||||
uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */
|
||||
float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */
|
||||
int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */
|
||||
float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */
|
||||
uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */
|
||||
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */
|
||||
float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */
|
||||
float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */
|
||||
float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */
|
||||
float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */
|
||||
uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */
|
||||
uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */
|
||||
bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */
|
||||
int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */
|
||||
uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */
|
||||
float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */
|
||||
|
||||
int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */
|
||||
uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */
|
||||
float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */
|
||||
int accelerometer_mode; /**< Accelerometer measurement mode */
|
||||
float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */
|
||||
|
||||
int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */
|
||||
float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */
|
||||
int magnetometer_mode; /**< Magnetometer measurement mode */
|
||||
float magnetometer_range_ga; /**< ± measurement range in Gauss */
|
||||
float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */
|
||||
uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */
|
||||
|
||||
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */
|
||||
float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */
|
||||
float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */
|
||||
float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */
|
||||
float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */
|
||||
uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */
|
||||
uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */
|
||||
bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */
|
||||
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user