diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-17 17:37:58 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-17 17:37:58 +0200 |
commit | bce043a21b7f39f786755fa3118c2c5e25eb8a94 (patch) | |
tree | 4a3df548c5e70c33d03d1ad9a813c99ca2958a27 | |
parent | 73286f3262b624ab62ce3f2d55e3521c1cb5f135 (diff) | |
download | px4-firmware-bce043a21b7f39f786755fa3118c2c5e25eb8a94.tar.gz px4-firmware-bce043a21b7f39f786755fa3118c2c5e25eb8a94.tar.bz2 px4-firmware-bce043a21b7f39f786755fa3118c2c5e25eb8a94.zip |
Fixed mag axis assignment, fixed mag calibration
-rw-r--r-- | apps/ardrone_control/Makefile | 2 | ||||
-rw-r--r-- | apps/commander/commander.c | 19 | ||||
-rw-r--r-- | apps/sensors/Makefile | 2 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 8 |
4 files changed, 22 insertions, 9 deletions
diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile index 2038e982b..a6d471567 100644 --- a/apps/ardrone_control/Makefile +++ b/apps/ardrone_control/Makefile @@ -37,7 +37,7 @@ APPNAME = ardrone_control PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 3096 +STACKSIZE = 4096 # explicit list of sources - not everything is built currently CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e290c2140..47829ddf5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -376,9 +376,22 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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]); float mag_offset[3]; - mag_offset[0] = (max_avg[0] - min_avg[0]); - mag_offset[1] = (max_avg[1] - min_avg[1]); - mag_offset[2] = (max_avg[2] - min_avg[2]); + + /** + * The offset is subtracted from the sensor values, so the result is the + * POSITIVE number that has to be subtracted from the sensor data + * to shift the center to zero + * + * offset = max - ((max - min) / 2.0f) + * + * which reduces to + * + * offset = (max + min) / 2.0f + */ + + mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f; + mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; + mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; 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]; diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index dadc2993d..125839bb3 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,6 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 5e93f180f..7933e8518 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -803,7 +803,7 @@ int sensors_main(int argc, char *argv[]) /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor - raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor + raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : buf_accelerometer[2]; // z of the board is z of the sensor // XXX read range from sensor float range_g = 4.0f; @@ -820,9 +820,9 @@ int sensors_main(int argc, char *argv[]) /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor - raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor - raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor + raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is -y of the sensor + raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is x of the sensor + raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f; |