Commit e18e5ca5 authored by hauptmech's avatar hauptmech Committed by Lorenz Meier
Browse files

Add SENS_MAG_ID parameter

Record devid to SENS_MAG_ID during mag calibration
Verify devid matches SENS_MAG_ID during preflight_check
parent 6899a516
......@@ -41,9 +41,11 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_device.h"
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define MAG_DEVICE_PATH "/dev/mag"
/**
......
......@@ -65,6 +65,7 @@ static const char *sensor_name = "mag";
int do_mag_calibration(int mavlink_fd)
{
int32_t device_id;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
......@@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd)
/* erase old calibration */
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (res != OK) {
......@@ -253,6 +257,9 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
......
......@@ -98,6 +98,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
/**
* ID of Magnetometer the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
/**
* Magnetometer X-axis offset
......
......@@ -58,6 +58,7 @@
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
#include "systemlib/param/param.h"
__EXPORT int config_main(int argc, char *argv[]);
......@@ -264,8 +265,11 @@ do_mag(int argc, char *argv[])
int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, MAGIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
param_get(param_find("SENS_MAG_ID"), &(calibration_id));
warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
close(fd);
}
......
......@@ -84,6 +84,7 @@ int preflight_check_main(int argc, char *argv[])
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
int32_t mag_devid,mag_calibration_devid;
/* give the system some time to sample the sensors in the background */
usleep(150000);
......@@ -96,6 +97,16 @@ int preflight_check_main(int argc, char *argv[])
system_ok = false;
goto system_eval;
}
mag_devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid));
if (mag_devid != mag_calibration_devid){
warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment