diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-09-27 18:43:04 +0200 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-09-27 18:43:04 +0200 |
commit | d206327541f159ac4abd76e66bce3160e8704231 (patch) | |
tree | 00fd9c26f9c0c15b198dc71b4f1a480348a8b46d | |
parent | 2c5c3141057be1f46cb4a33f71e2331ce36b18a7 (diff) | |
download | px4-firmware-d206327541f159ac4abd76e66bce3160e8704231.tar.gz px4-firmware-d206327541f159ac4abd76e66bce3160e8704231.tar.bz2 px4-firmware-d206327541f159ac4abd76e66bce3160e8704231.zip |
Magnetometer calibration fixed
-rw-r--r-- | apps/commander/commander.c | 55 |
1 files changed, 15 insertions, 40 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1c23c1f9d..960c5d383 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -256,22 +256,6 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -static void cal_bsort(float 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; - } - } - } -} - static const char *parameter_file = "/eeprom/parameters"; static int pm_save_eeprom(bool only_unsaved) @@ -312,8 +296,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - float mag_max[3] = {0, 0, 0}; - float mag_min[3] = {0, 0, 0}; + float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -340,25 +324,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - if (raw.magnetometer_raw[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_raw[0]; + if (raw.magnetometer_ga[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_ga[0]; } - else if (raw.magnetometer_raw[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_raw[0]; + else if (raw.magnetometer_ga[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_ga[0]; } - if (raw.magnetometer_raw[1] < mag_min[1]) { - mag_min[1] = raw.magnetometer_raw[1]; + if (raw.magnetometer_ga[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_ga[1]; } - else if (raw.magnetometer_raw[1] > mag_max[1]) { - mag_max[1] = raw.magnetometer_raw[1]; + else if (raw.magnetometer_ga[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_ga[1]; } - if (raw.magnetometer_raw[2] < mag_min[2]) { - mag_min[2] = raw.magnetometer_raw[2]; + if (raw.magnetometer_ga[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_ga[2]; } - else if (raw.magnetometer_raw[2] > mag_max[2]) { - mag_max[2] = raw.magnetometer_raw[2]; + else if (raw.magnetometer_ga[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_ga[2]; } calibration_counter++; @@ -387,20 +371,11 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); - mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); /* announce and set new offset */ |