dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed mag axis assignment, fixed mag calibration

This commit is contained in:
Lorenz Meier 2012-08-17 17:37:58 +02:00
parent 73286f3262
commit bce043a21b
4 changed files with 22 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/ardrone_control/Makefile
View File

@ -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

19
apps/commander/commander.c
View File

@ -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];

2
apps/sensors/Makefile
View File

@ -37,6 +37,6 @@
APPNAME = sensors
PRIORITY = SCHED_PRIORITY_MAX-5
STACKSIZE = 2048
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk

8
apps/sensors/sensors.c
View File

@ -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;