aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/drv_accel.h2
-rw-r--r--src/drivers/drv_gyro.h2
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp3
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp3
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp20
-rw-r--r--src/modules/commander/accelerometer_calibration.cpp16
-rw-r--r--src/modules/commander/gyro_calibration.cpp24
-rw-r--r--src/modules/commander/mag_calibration.cpp16
-rw-r--r--src/modules/navigator/navigator_main.cpp7
-rw-r--r--src/modules/sensors/sensor_params.c386
-rw-r--r--src/modules/sensors/sensors.cpp181
-rw-r--r--src/modules/systemlib/mcu_version.h4
-rw-r--r--src/systemcmds/config/config.c6
-rw-r--r--src/systemcmds/preflight_check/preflight_check.c6
14 files changed, 514 insertions, 162 deletions
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 1eca6155b..52e024c91 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -97,6 +97,8 @@ ORB_DECLARE(sensor_accel);
/** set the accel internal sample rate to at least (arg) Hz */
#define ACCELIOCSSAMPLERATE _ACCELIOC(0)
+#define ACCEL_SAMPLERATE_DEFAULT 1000003 /**< default sample rate */
+
/** return the accel internal sample rate in Hz */
#define ACCELIOCGSAMPLERATE _ACCELIOC(1)
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 5e0334a18..1f2bc35c4 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -93,6 +93,8 @@ ORB_DECLARE(sensor_gyro);
/** set the gyro internal sample rate to at least (arg) Hz */
#define GYROIOCSSAMPLERATE _GYROIOC(0)
+#define GYRO_SAMPLERATE_DEFAULT 1000003 /**< default sample rate */
+
/** return the gyro internal sample rate in Hz */
#define GYROIOCGSAMPLERATE _GYROIOC(1)
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index f583bced4..547bb6868 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -784,8 +784,9 @@ L3GD20::set_samplerate(unsigned frequency)
{
uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
- if (frequency == 0)
+ if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) {
frequency = _is_l3g4200d ? 800 : 760;
+ }
/*
* Use limits good for H or non-H models. Rates are slightly different
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 2a0bf522b..6edb9d787 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1307,8 +1307,9 @@ LSM303D::accel_set_samplerate(unsigned frequency)
uint8_t setbits = 0;
uint8_t clearbits = REG1_RATE_BITS_A;
- if (frequency == 0)
+ if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
frequency = 1600;
+ }
if (frequency <= 100) {
setbits |= REG1_RATE_100HZ_A;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index e322e8b3a..1c4dfb89e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -402,7 +402,7 @@ private:
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
- void _set_sample_rate(uint16_t desired_sample_rate_hz);
+ void _set_sample_rate(unsigned desired_sample_rate_hz);
/*
check that key registers still have the right value
@@ -794,13 +794,19 @@ MPU6000::probe()
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
*/
void
-MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz)
{
- uint8_t div = 1000 / desired_sample_rate_hz;
- if(div>200) div=200;
- if(div<1) div=1;
- write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
- _sample_rate = 1000 / div;
+ if (desired_sample_rate_hz == 0 ||
+ desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT ||
+ desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) {
+ desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE;
+ }
+
+ uint8_t div = 1000 / desired_sample_rate_hz;
+ if(div>200) div=200;
+ if(div<1) div=1;
+ write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
+ _sample_rate = 1000 / div;
}
/*
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 13ab966ab..d9e7e21fc 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -221,17 +221,17 @@ int do_accel_calibration(int mavlink_fd)
accel_scale.z_scale = accel_T_rotated(2, 2);
/* set parameters */
- if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
- || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
- || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
- || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
- || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
- || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
+ if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset))
+ || param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset))
+ || param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset))
+ || param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale))
+ || param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale))
+ || param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
res = ERROR;
}
- if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+ if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
res = ERROR;
}
}
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 8410297ef..e941e327c 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -51,6 +51,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -80,6 +81,13 @@ int do_gyro_calibration(int mavlink_fd)
int res = OK;
+ /* store board ID */
+ uint32_t mcu_id[3];
+ mcu_unique_id(&mcu_id[0]);
+
+ /* store last 32bit number - not unique, but unique in a given set */
+ param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
+
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
@@ -149,9 +157,9 @@ int do_gyro_calibration(int mavlink_fd)
if (res == OK) {
/* set offset parameters to new values */
- if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
- || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
- || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+ if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset))
+ || param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset))
+ || param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
res = ERROR;
}
@@ -275,13 +283,13 @@ int do_gyro_calibration(int mavlink_fd)
if (res == OK) {
/* set scale parameters to new values */
- if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
- || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
- || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+ if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
+ || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale))
+ || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
res = ERROR;
}
- if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
+ if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) {
res = ERROR;
}
}
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 2afb9a440..7147fb283 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -263,30 +263,30 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
- if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
+ if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
res = ERROR;
}
- if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
+ if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
- if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
+ if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
res = ERROR;
}
- if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
+ if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
- if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
+ if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
- if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
+ if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
- if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+ if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e35b0bd6a..ad2349c94 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -247,9 +247,6 @@ Navigator::task_main_trampoline(int argc, char *argv[])
void
Navigator::task_main()
{
- /* inform about start */
- warnx("Initializing..");
-
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
@@ -263,7 +260,7 @@ Navigator::task_main()
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
- mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
+ mavlink_log_info(_mavlink_fd, "No geofence set");
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 67b7feef7..5e04241fe 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,20 +36,192 @@
*
* Parameters defined by the sensors task.
*
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@px4.io>
+ * @author Thomas Gubler <thomas@px4.io>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
+ * ID of the board this parameter set was calibrated on.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_BOARD_ID, 0);
+
+/**
+ * ID of the Gyro that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
+
+/**
+ * Gyro X-axis offset
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0f);
+
+/**
+ * Gyro Y-axis offset
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f);
+
+/**
+ * Gyro Z-axis offset
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f);
+
+/**
+ * Gyro X-axis scaling factor
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_XSCALE, 1.0f);
+
+/**
+ * Gyro Y-axis scaling factor
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_YSCALE, 1.0f);
+
+/**
+ * Gyro Z-axis scaling factor
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_ZSCALE, 1.0f);
+
+/**
+ * ID of Magnetometer the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
+
+/**
+ * Magnetometer X-axis offset
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0f);
+
+/**
+ * Magnetometer Y-axis offset
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0f);
+
+/**
+ * Magnetometer Z-axis offset
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0f);
+
+/**
+ * Magnetometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0f);
+
+/**
+ * Magnetometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
+
+/**
+ * Magnetometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
+
+/**
+ * ID of the Accelerometer that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
+
+/**
+ * Accelerometer X-axis offset
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0f);
+
+/**
+ * Accelerometer Y-axis offset
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0f);
+
+/**
+ * Accelerometer Z-axis offset
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0f);
+
+/**
+ * Accelerometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0f);
+
+/**
+ * Accelerometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0f);
+
+/**
+ * Accelerometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f);
+
+/**
* ID of the Gyro that the calibration is for.
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
+PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
/**
* Gyro X-axis offset
@@ -58,7 +230,7 @@ PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
* @max 10.0
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0f);
/**
* Gyro Y-axis offset
@@ -67,7 +239,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
* @max 10.0
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f);
/**
* Gyro Z-axis offset
@@ -76,7 +248,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
* @max 5.0
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
@@ -85,7 +257,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
* @max 1.5
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
@@ -94,7 +266,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
* @max 1.5
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
@@ -103,14 +275,14 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
* @max 1.5
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f);
/**
* ID of Magnetometer the calibration is for.
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
+PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
/**
* Magnetometer X-axis offset
@@ -119,7 +291,7 @@ PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
* @max 500.0
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0f);
/**
* Magnetometer Y-axis offset
@@ -128,7 +300,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
* @max 500.0
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0f);
/**
* Magnetometer Z-axis offset
@@ -137,78 +309,242 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
* @max 500.0
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0f);
/**
* Magnetometer X-axis scaling factor
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0f);
/**
* Magnetometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
/**
* Magnetometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
/**
* ID of the Accelerometer that the calibration is for.
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_INT32(SENS_ACC_ID, 0);
+PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
/**
* Accelerometer X-axis offset
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0f);
/**
* Accelerometer Y-axis offset
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0f);
/**
* Accelerometer Z-axis offset
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
+PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0f);
/**
* Accelerometer X-axis scaling factor
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0f);
/**
* Accelerometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0f);
/**
* Accelerometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f);
+
+/**
+ * ID of the Gyro that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
+
+/**
+ * Gyro X-axis offset
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0f);
+
+/**
+ * Gyro Y-axis offset
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f);
+
+/**
+ * Gyro Z-axis offset
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f);
+
+/**
+ * Gyro X-axis scaling factor
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_XSCALE, 1.0f);
+
+/**
+ * Gyro Y-axis scaling factor
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_YSCALE, 1.0f);
+
+/**
+ * Gyro Z-axis scaling factor
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f);
+
+/**
+ * ID of Magnetometer the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
+
+/**
+ * Magnetometer X-axis offset
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0f);
+
+/**
+ * Magnetometer Y-axis offset
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0f);
+/**
+ * Magnetometer Z-axis offset
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0f);
+
+/**
+ * Magnetometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0f);
+
+/**
+ * Magnetometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
+
+/**
+ * Magnetometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
+
+/**
+ * ID of the Accelerometer that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
+
+/**
+ * Accelerometer X-axis offset
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0f);
+
+/**
+ * Accelerometer Y-axis offset
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0f);
+
+/**
+ * Accelerometer Z-axis offset
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0f);
+
+/**
+ * Accelerometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0f);
+
+/**
+ * Accelerometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
+
+/**
+ * Accelerometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
/**
* Differential pressure sensor offset
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 867d6c339..87d7da537 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -512,7 +512,7 @@ Sensors::Sensors() :
_hil_enabled(false),
_publishing(true),
-/* subscriptions */
+ /* subscriptions */
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
@@ -530,7 +530,7 @@ Sensors::Sensors() :
_rc_parameter_map_sub(-1),
_manual_control_sub(-1),
-/* publications */
+ /* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
_actuator_group_3_pub(-1),
@@ -539,7 +539,7 @@ Sensors::Sensors() :
_airspeed_pub(-1),
_diff_pres_pub(-1),
-/* performance counters */
+ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_mag_is_external(false),
@@ -619,29 +619,29 @@ Sensors::Sensors() :
_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
/* gyro offsets */
- _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
- _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
- _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
- _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
- _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
- _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");
+ _parameter_handles.gyro_offset[0] = param_find("CAL_GYRO0_XOFF");
+ _parameter_handles.gyro_offset[1] = param_find("CAL_GYRO0_YOFF");
+ _parameter_handles.gyro_offset[2] = param_find("CAL_GYRO0_ZOFF");
+ _parameter_handles.gyro_scale[0] = param_find("CAL_GYRO0_XSCALE");
+ _parameter_handles.gyro_scale[1] = param_find("CAL_GYRO0_YSCALE");
+ _parameter_handles.gyro_scale[2] = param_find("CAL_GYRO0_ZSCALE");
/* accel offsets */
- _parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
- _parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF");
- _parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF");
- _parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE");
- _parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE");
- _parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE");
+ _parameter_handles.accel_offset[0] = param_find("CAL_ACC0_XOFF");
+ _parameter_handles.accel_offset[1] = param_find("CAL_ACC0_YOFF");
+ _parameter_handles.accel_offset[2] = param_find("CAL_ACC0_ZOFF");
+ _parameter_handles.accel_scale[0] = param_find("CAL_ACC0_XSCALE");
+ _parameter_handles.accel_scale[1] = param_find("CAL_ACC0_YSCALE");
+ _parameter_handles.accel_scale[2] = param_find("CAL_ACC0_ZSCALE");
/* mag offsets */
- _parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF");
- _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
- _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");
+ _parameter_handles.mag_offset[0] = param_find("CAL_MAG0_XOFF");
+ _parameter_handles.mag_offset[1] = param_find("CAL_MAG0_YOFF");
+ _parameter_handles.mag_offset[2] = param_find("CAL_MAG0_ZOFF");
- _parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE");
- _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
- _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
+ _parameter_handles.mag_scale[0] = param_find("CAL_MAG0_XSCALE");
+ _parameter_handles.mag_scale[1] = param_find("CAL_MAG0_YSCALE");
+ _parameter_handles.mag_scale[2] = param_find("CAL_MAG0_ZSCALE");
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
@@ -786,9 +786,11 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
+
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
}
+
param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
@@ -883,15 +885,18 @@ Sensors::parameters_update()
/* set px4flow rotation */
int flowfd;
flowfd = open(PX4FLOW_DEVICE_PATH, 0);
+
if (flowfd >= 0) {
- int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
- if (flowret) {
- warnx("flow rotation could not be set");
+ int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
+
+ if (flowret) {
+ warnx("flow rotation could not be set");
close(flowfd);
return ERROR;
- }
+ }
+
close(flowfd);
- }
+ }
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
@@ -902,9 +907,9 @@ Sensors::parameters_update()
/** fine tune board offset on parameter update **/
math::Matrix<3, 3> board_rotation_offset;
- board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
- M_DEG_TO_RAD_F * _parameters.board_offset[1],
- M_DEG_TO_RAD_F * _parameters.board_offset[2]);
+ board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
+ M_DEG_TO_RAD_F * _parameters.board_offset[1],
+ M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
@@ -912,17 +917,20 @@ Sensors::parameters_update()
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
int barofd;
barofd = open(BARO_DEVICE_PATH, 0);
+
if (barofd < 0) {
warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH);
return ERROR;
} else {
int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+
if (baroret) {
warnx("qnh could not be set");
close(barofd);
return ERROR;
}
+
close(barofd);
}
@@ -942,28 +950,11 @@ Sensors::accel_init()
} else {
- // XXX do the check more elegantly
-
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ /* set the accel internal sampling rate to default rate */
+ ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
- /* set the accel internal sampling rate up to at leat 1000Hz */
- ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
-
- /* set the driver to poll at 1000Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 1000);
-
-#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE
-
- /* set the accel internal sampling rate up to at leat 800Hz */
- ioctl(fd, ACCELIOCSSAMPLERATE, 800);
-
- /* set the driver to poll at 800Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 800);
-
-#else
-#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE
-
-#endif
+ /* set the driver to poll at default rate */
+ ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
}
@@ -984,31 +975,12 @@ Sensors::gyro_init()
} else {
- // XXX do the check more elegantly
+ /* set the gyro internal sampling rate to default rate */
+ ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ /* set the driver to poll at default rate */
+ ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
- /* set the gyro internal sampling rate up to at least 1000Hz */
- if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) {
- ioctl(fd, GYROIOCSSAMPLERATE, 800);
- }
-
- /* set the driver to poll at 1000Hz */
- if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) {
- ioctl(fd, SENSORIOCSPOLLRATE, 800);
- }
-
-#else
-
- /* set the gyro internal sampling rate up to at least 760Hz */
- ioctl(fd, GYROIOCSSAMPLERATE, 760);
-
- /* set the driver to poll at 760Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 760);
-
-#endif
-
- close(fd);
}
return OK;
@@ -1348,14 +1320,17 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
- float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+ float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
+ (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
/* don't risk to feed negative airspeed into the system */
- _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
- _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
- raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
+ _airspeed.indicated_airspeed_m_s = math::max(0.0f,
+ calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
+ _airspeed.true_airspeed_m_s = math::max(0.0f,
+ calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+ raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
@@ -1479,8 +1454,10 @@ Sensors::parameter_update_poll(bool forced)
}
#if 0
- printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
- printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
+ printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0],
+ (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
+ printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1],
+ (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100));
fflush(stdout);
usleep(5000);
@@ -1496,6 +1473,7 @@ Sensors::rc_parameter_map_poll(bool forced)
if (map_updated) {
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
+
/* update paramter handles to which the RC channels are mapped */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
@@ -1508,21 +1486,24 @@ Sensors::rc_parameter_map_poll(bool forced)
/* Set the handle by index if the index is set, otherwise use the id */
if (_rc_parameter_map.param_index[i] >= 0) {
_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
+
} else {
_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
}
}
+
warnx("rc to parameter map updated");
+
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
- i,
- _rc_parameter_map.param_id[i],
- (double)_rc_parameter_map.scale[i],
- (double)_rc_parameter_map.value0[i],
- (double)_rc_parameter_map.value_min[i],
- (double)_rc_parameter_map.value_max[i]
- );
+ i,
+ _rc_parameter_map.param_id[i],
+ (double)_rc_parameter_map.scale[i],
+ (double)_rc_parameter_map.value0[i],
+ (double)_rc_parameter_map.value_min[i],
+ (double)_rc_parameter_map.value_max[i]
+ );
}
}
}
@@ -1616,7 +1597,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
- _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
+ _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
+ (diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
/* announce the airspeed if needed, just publish else */
@@ -1709,6 +1691,7 @@ void
Sensors::set_params_from_rc()
{
static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
+
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
@@ -1717,14 +1700,14 @@ Sensors::set_params_from_rc()
continue;
}
- float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
+ float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
param_rc_values[i] = rc_val;
float param_val = math::constrain(
- _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
- _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
+ _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
+ _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
param_set(_parameter_handles.rc_param[i], &param_val);
}
}
@@ -1808,10 +1791,12 @@ Sensors::rc_poll()
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
- _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
+ _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(
+ _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
- _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
+ _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(
+ _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
@@ -1899,6 +1884,7 @@ Sensors::rc_poll()
/* Update parameters from RC Channels (tuning with RC) if activated */
static hrt_abstime last_rc_to_param_map_time = 0;
+
if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) {
set_params_from_rc();
last_rc_to_param_map_time = hrt_absolute_time();
@@ -1920,22 +1906,31 @@ Sensors::task_main()
/* start individual sensors */
int ret;
ret = accel_init();
+
if (ret) {
goto exit_immediate;
}
+
ret = gyro_init();
+
if (ret) {
goto exit_immediate;
}
+
ret = mag_init();
+
if (ret) {
goto exit_immediate;
}
+
ret = baro_init();
+
if (ret) {
goto exit_immediate;
}
+
ret = adc_init();
+
if (ret) {
goto exit_immediate;
}
@@ -2075,7 +2070,7 @@ Sensors::start()
nullptr);
/* wait until the task is up and running or has failed */
- while(_sensors_task > 0 && _task_should_exit) {
+ while (_sensors_task > 0 && _task_should_exit) {
usleep(100);
}
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
index c8a0bf5cd..e951e1e39 100644
--- a/src/modules/systemlib/mcu_version.h
+++ b/src/modules/systemlib/mcu_version.h
@@ -35,6 +35,8 @@
#include <stdint.h>
+__BEGIN_DECLS
+
/* magic numbers from reference manual */
enum MCU_REV {
MCU_REV_STM32F4_REV_A = 0x1000,
@@ -61,3 +63,5 @@ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit);
* @return The silicon revision / version number as integer
*/
__EXPORT int mcu_version(char* rev, char** revstr);
+
+__END_DECLS
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index f54342f06..78db19801 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -195,7 +195,7 @@ do_gyro(int argc, char *argv[])
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
- param_get(param_find("SENS_GYRO_ID"), &(calibration_id));
+ param_get(param_find("CAL_GYRO0_ID"), &(calibration_id));
warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range);
@@ -272,7 +272,7 @@ do_mag(int argc, char *argv[])
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
- param_get(param_find("SENS_MAG_ID"), &(calibration_id));
+ param_get(param_find("CAL_MAG0_ID"), &(calibration_id));
warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
@@ -349,7 +349,7 @@ do_accel(int argc, char *argv[])
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
- param_get(param_find("SENS_ACC_ID"), &(calibration_id));
+ param_get(param_find("CAL_ACC0_ID"), &(calibration_id));
warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range);
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 3e1f76714..c5c959cf3 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -99,7 +99,7 @@ int preflight_check_main(int argc, char *argv[])
}
devid = ioctl(fd, DEVIOCGDEVICEID,0);
- param_get(param_find("SENS_MAG_ID"), &(calibration_devid));
+ param_get(param_find("CAL_MAG0_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
@@ -122,7 +122,7 @@ int preflight_check_main(int argc, char *argv[])
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
devid = ioctl(fd, DEVIOCGDEVICEID,0);
- param_get(param_find("SENS_ACC_ID"), &(calibration_devid));
+ param_get(param_find("CAL_ACC0_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("accelerometer calibration is for a different device - calibrate accelerometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID");
@@ -168,7 +168,7 @@ int preflight_check_main(int argc, char *argv[])
fd = open(GYRO_DEVICE_PATH, 0);
devid = ioctl(fd, DEVIOCGDEVICEID,0);
- param_get(param_find("SENS_GYRO_ID"), &(calibration_devid));
+ param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("gyro calibration is for a different device - calibrate gyro first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID");
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 21:24:41 +0000