diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a80ca4eb64..3e21ec2a91 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -122,6 +122,12 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); /** * Rotation of magnetometer 0 relative to airframe. * + * An internal magnetometer will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @min -1 + * @max 30 * @group Sensor Calibration */ PARAM_DEFINE_INT32(CAL_MAG0_ROT, 0); @@ -292,8 +298,14 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f); PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); /** - * Rotation of magnetometer 0 relative to airframe. + * Rotation of magnetometer 1 relative to airframe. * + * An internal magnetometer will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @min -1 + * @max 30 * @group Sensor Calibration */ PARAM_DEFINE_INT32(CAL_MAG1_ROT, 0); @@ -464,8 +476,14 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f); PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); /** - * Rotation of magnetometer 0 relative to airframe. + * Rotation of magnetometer 2 relative to airframe. * + * An internal magnetometer will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @min -1 + * @max 30 * @group Sensor Calibration */ PARAM_DEFINE_INT32(CAL_MAG2_ROT, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f9a607d77d..7d60bf563b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -134,16 +134,16 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 #endif -#define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 4.8f +#define BATT_V_LOWPASS 0.001f +#define BATT_V_IGNORE_THRESHOLD 4.8f /** * HACK - true temperature is much less than indicated temperature in baro, * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f - -#define STICK_ON_OFF_LIMIT 0.75f +#define PCB_TEMP_ESTIMATE_DEG 5.0f +#define STICK_ON_OFF_LIMIT 0.75f +#define MAG_ROT_VAL_INTERNAL -1 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -1515,6 +1515,9 @@ Sensors::parameter_update_poll(bool forced) if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { /* mag is internal */ _mag_rotation[s] = _board_rotation; + /* reset param to -1 to indicate external mag */ + int32_t minus_one = MAG_ROT_VAL_INTERNAL; + param_set(param_find("CAL_MAG0_ROT"), &minus_one); } else { int32_t mag_rot = 0; @@ -1527,12 +1530,17 @@ Sensors::parameter_update_poll(bool forced) param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); /* if the old param is non-zero, set the new one to the same value */ - if ((deprecated_mag_rot != 0) && (mag_rot == 0)) { + if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) { mag_rot = deprecated_mag_rot; param_set(param_find("CAL_MAG0_ROT"), &mag_rot); } } + /* handling of transition from internal to external */ + if (mag_rot < 0) { + mag_rot = 0; + } + get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[s]); }