From 9a750ae6988814a07975e28368d949292710589f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Aug 2012 11:30:30 +0200 Subject: [PATCH 1/3] Correct scaling for calibration routines --- apps/commander/commander.c | 55 +++++++++++++++++++------------------- 1 file changed, 27 insertions(+), 28 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b2ddc8b45..07958032e0 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -271,25 +271,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* Get rid of 10% */ const int outlier_margin = (peak_samples) / 10; - int16_t *mag_maxima[3]; - mag_maxima[0] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); - mag_maxima[1] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); - mag_maxima[2] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); + float *mag_maxima[3]; + mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float)); + mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float)); + mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float)); - int16_t *mag_minima[3]; - mag_minima[0] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); - mag_minima[1] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); - mag_minima[2] = (int16_t*)malloc(peak_samples * sizeof(uint16_t)); + float *mag_minima[3]; + mag_minima[0] = (float*)malloc(peak_samples * sizeof(float)); + mag_minima[1] = (float*)malloc(peak_samples * sizeof(float)); + mag_minima[2] = (float*)malloc(peak_samples * sizeof(float)); /* 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_maxima[0][i] = FLT_MIN; + mag_maxima[1][i] = FLT_MIN; + mag_maxima[2][i] = FLT_MIN; - mag_minima[0][i] = INT16_MAX; - mag_minima[1][i] = INT16_MAX; - mag_minima[2][i] = INT16_MAX; + mag_minima[0][i] = FLT_MAX; + mag_minima[1][i] = FLT_MAX; + mag_minima[2][i] = FLT_MAX; } mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions."); @@ -308,23 +308,23 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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]; + mag_minima[0][i] = raw.magnetometer_ga[0]; /* y minimum */ if (raw.magnetometer_raw[1] < mag_minima[1][i]) - mag_minima[1][i] = raw.magnetometer_raw[1]; + mag_minima[1][i] = raw.magnetometer_ga[1]; /* z minimum */ if (raw.magnetometer_raw[2] < mag_minima[2][i]) - mag_minima[2][i] = raw.magnetometer_raw[2]; + mag_minima[2][i] = raw.magnetometer_ga[2]; /* x maximum */ if (raw.magnetometer_raw[0] > mag_maxima[0][i]) - mag_maxima[0][i] = raw.magnetometer_raw[0]; + mag_maxima[0][i] = raw.magnetometer_ga[0]; /* y maximum */ if (raw.magnetometer_raw[1] > mag_maxima[1][i]) - mag_maxima[1][i] = raw.magnetometer_raw[1]; + mag_maxima[1][i] = raw.magnetometer_ga[1]; /* z maximum */ if (raw.magnetometer_raw[2] > mag_maxima[2][i]) - mag_maxima[2][i] = raw.magnetometer_raw[2]; + mag_maxima[2][i] = raw.magnetometer_ga[2]; } calibration_counter++; @@ -354,7 +354,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) printf("start:\n"); for (int i = 0; i < 10; i++) { - printf("mag min: %d\t%d\t%d\tmag max: %d\t%d\t%d\n", + printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", mag_minima[0][i], mag_minima[1][i], mag_minima[2][i], @@ -366,7 +366,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) printf("-----\n"); for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { - printf("mag min: %d\t%d\t%d\tmag max: %d\t%d\t%d\n", + printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", mag_minima[0][i], mag_minima[1][i], mag_minima[2][i], @@ -397,7 +397,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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]); + printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], + (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); float mag_offset[3]; @@ -465,9 +466,9 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) if (poll(fds, 1, 1000)) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_raw[0]; - gyro_offset[1] += raw.gyro_raw[1]; - gyro_offset[2] += raw.gyro_raw[2]; + 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++; } else { /* any poll failure for 1s is a reason to abort */ @@ -501,8 +502,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, offset_output); close(sub_sensor_combined); - - // XXX Add a parameter changed broadcast notification } From d8210a8e2f35bb3260875968424a7985012766a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Aug 2012 11:30:45 +0200 Subject: [PATCH 2/3] Implemented missing IOCTLs to set MPU scalings --- apps/drivers/mpu6000/mpu6000.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7c5ff2ee06..83bd138601 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -589,9 +589,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case ACCELIOCSSCALE: + /* copy scale in */ + memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + return OK; + case ACCELIOCGSCALE: - /* XXX not implemented */ - return -EINVAL; + /* copy scale out */ + memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; case ACCELIOCSRANGE: case ACCELIOCGRANGE: @@ -631,9 +636,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + return OK; + case GYROIOCGSCALE: - /* XXX not implemented */ - return -EINVAL; + /* copy scale out */ + memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; case GYROIOCSRANGE: case GYROIOCGRANGE: From fa321849737fb7e5423e118bbbcd28a014e1d0a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Aug 2012 13:56:39 +0200 Subject: [PATCH 3/3] params debugging --- apps/commander/commander.c | 8 +++---- apps/sensors/sensors.cpp | 47 +++++++++++++++++++++++++++++--------- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 07958032e0..09da50480e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -746,12 +746,12 @@ float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage initialized = true; } - float chemistry_voltage_empty[1]; - float chemistry_voltage_full[1]; + float chemistry_voltage_empty[1] = { 3.2f }; + float chemistry_voltage_full[1] = { 4.05f }; if (counter % 100 == 0) { - param_get(bat_volt_empty, chemistry_voltage_empty+0); - param_get(bat_volt_full, chemistry_voltage_full+0); + param_get(bat_volt_empty, &(chemistry_voltage_empty[0])); + param_get(bat_volt_full, &(chemistry_voltage_full[0])); } counter++; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 98b4768148..9fb42bdf67 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -428,33 +428,53 @@ Sensors::parameters_update() /* min values */ for (unsigned int i = 0; i < nchans; i++) { - param_get(_parameter_handles.min[i], &(_parameters.min[i])); + if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { + warnx("Failed getting min for chan %d", i); + } } /* trim values */ for (unsigned int i = 0; i < nchans; i++) { - param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); + if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) { + warnx("Failed getting trim for chan %d", i); + } } /* max values */ for (unsigned int i = 0; i < nchans; i++) { - param_get(_parameter_handles.max[i], &(_parameters.max[i])); + if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) { + warnx("Failed getting max for chan %d", i); + } } /* channel reverse */ for (unsigned int i = 0; i < nchans; i++) { - param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); + if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) { + warnx("Failed getting rev for chan %d", i); + } } /* remote control type */ - param_get(_parameter_handles.rc_type, &(_parameters.rc_type)); + if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { + warnx("Failed getting remote control type"); + } /* channel mapping */ - param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)); - param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)); - param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)); - param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)); - param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)); + if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { + warnx("Failed getting roll chan index"); + } + if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { + warnx("Failed getting pitch chan index"); + } + if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { + warnx("Failed getting yaw chan index"); + } + if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { + warnx("Failed getting throttle chan index"); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); + } /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -820,6 +840,11 @@ Sensors::parameter_update_poll() _rc.function[2] = _parameters.rc_map_pitch - 1; _rc.function[3] = _parameters.rc_map_yaw - 1; _rc.function[4] = _parameters.rc_map_mode_sw - 1; + + printf("RAW S: %8.4f MID: %d R: %d P: %d\n", _rc.chan[0].scaling_factor, (int)_rc.chan[0].mid, (int)_rc.function[0], (int)_rc.function[1]); + printf("RAW MAN: %8.4f %8.4f\n", _rc.chan[0].scaled, _rc.chan[1].scaled); + fflush(stdout); + usleep(5000); } } @@ -870,7 +895,7 @@ Sensors::ppm_poll() if (_ppm_last_valid == ppm_last_valid_decode) return; /* require at least two chanels to consider the signal valid */ - if (ppm_decoded_channels < 2) + if (ppm_decoded_channels < 4) return; /* we are accepting this decode */