diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-05 22:51:31 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-05 22:51:31 +0200 |
commit | 96b348af9f78dc7ead79224c921fb7480aff168e (patch) | |
tree | a5ff26f84812a2b121dd3f1dd624e82872f527aa /apps | |
parent | 139cd091768c57272fe1f80d725d4a3a24d2e3d0 (diff) | |
download | px4-firmware-96b348af9f78dc7ead79224c921fb7480aff168e.tar.gz px4-firmware-96b348af9f78dc7ead79224c921fb7480aff168e.tar.bz2 px4-firmware-96b348af9f78dc7ead79224c921fb7480aff168e.zip |
Minor fixes to HMC driver, mag calibration done
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 202 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 34 | ||||
-rw-r--r-- | apps/uORB/topics/sensor_combined.h | 50 |
3 files changed, 246 insertions, 40 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f342298ee..1c83dd3f3 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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 do_gyro_calibration(void) +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; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 3343b33f4..7e8eac453 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -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++; } diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index ecf5ea81d..8fae78858 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -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 */ + int accelerometer_mode; /**< Accelerometer measurement mode */ + float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ - 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 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 */ }; |