aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sensors
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sensors')
-rw-r--r--src/modules/sensors/sensor_params.c301
-rw-r--r--src/modules/sensors/sensors.cpp62
2 files changed, 299 insertions, 64 deletions
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 30659fd3a..288c6e00a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -42,13 +42,10 @@
*/
#include <nuttx/config.h>
-
#include <systemlib/param/param.h>
/**
- * Gyro X offset
- *
- * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
+ * Gyro X-axis offset
*
* @min -10.0
* @max 10.0
@@ -57,7 +54,7 @@
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
/**
- * Gyro Y offset
+ * Gyro Y-axis offset
*
* @min -10.0
* @max 10.0
@@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
/**
- * Gyro Z offset
+ * Gyro Z-axis offset
*
* @min -5.0
* @max 5.0
@@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
/**
- * Gyro X scaling
- *
- * X-axis scaling.
+ * Gyro X-axis scaling factor
*
* @min -1.5
* @max 1.5
@@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
/**
- * Gyro Y scaling
- *
- * Y-axis scaling.
+ * Gyro Y-axis scaling factor
*
* @min -1.5
* @max 1.5
@@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
/**
- * Gyro Z scaling
- *
- * Z-axis scaling.
+ * Gyro Z-axis scaling factor
*
* @min -1.5
* @max 1.5
@@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
+
/**
- * Magnetometer X offset
- *
- * This is an X-axis offset for the magnetometer.
+ * Magnetometer X-axis offset
*
* @min -500.0
* @max 500.0
@@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
/**
- * Magnetometer Y offset
- *
- * This is an Y-axis offset for the magnetometer.
+ * Magnetometer Y-axis offset
*
* @min -500.0
* @max 500.0
@@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
/**
- * Magnetometer Z offset
- *
- * This is an Z-axis offset for the magnetometer.
+ * Magnetometer Z-axis offset
*
* @min -500.0
* @max 500.0
@@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
+/**
+ * Magnetometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
+
+/**
+ * Magnetometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
+
+/**
+ * Magnetometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
+
+/**
+ * Accelerometer X-axis offset
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
+
+/**
+ * Accelerometer Y-axis offset
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
+
+/**
+ * Accelerometer Z-axis offset
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
+/**
+ * Accelerometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
+
+/**
+ * Accelerometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
+
+/**
+ * Accelerometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
+
+/**
+ * Differential pressure sensor offset
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+
+/**
+ * Differential pressure sensor analog enabled
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
+
+/**
+ * Board rotation
+ *
+ * This parameter defines the rotation of the FMU board relative to the platform.
+ * Possible values are:
+ * 0 = No rotation
+ * 1 = Yaw 45°
+ * 2 = Yaw 90°
+ * 3 = Yaw 135°
+ * 4 = Yaw 180°
+ * 5 = Yaw 225°
+ * 6 = Yaw 270°
+ * 7 = Yaw 315°
+ * 8 = Roll 180°
+ * 9 = Roll 180°, Yaw 45°
+ * 10 = Roll 180°, Yaw 90°
+ * 11 = Roll 180°, Yaw 135°
+ * 12 = Pitch 180°
+ * 13 = Roll 180°, Yaw 225°
+ * 14 = Roll 180°, Yaw 270°
+ * 15 = Roll 180°, Yaw 315°
+ * 16 = Roll 90°
+ * 17 = Roll 90°, Yaw 45°
+ * 18 = Roll 90°, Yaw 90°
+ * 19 = Roll 90°, Yaw 135°
+ * 20 = Roll 270°
+ * 21 = Roll 270°, Yaw 45°
+ * 22 = Roll 270°, Yaw 90°
+ * 23 = Roll 270°, Yaw 135°
+ * 24 = Pitch 90°
+ * 25 = Pitch 270°
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+
+/**
+ * External magnetometer rotation
+ *
+ * This parameter defines the rotation of the external magnetometer relative
+ * to the platform (not relative to the FMU).
+ * See SENS_BOARD_ROT for possible values.
+ *
+ * @group Sensor Calibration
+ */
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
+
/**
* RC Channel 1 Minimum
*
@@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
#endif
-PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
+/**
+ * DSM binding trigger.
+ *
+ * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind
+ *
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
+
+
+/**
+ * Scaling factor for battery voltage sensor on PX4IO.
+ *
+ * @group Battery Calibration
+ */
PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
+
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/**
+ * Scaling factor for battery voltage sensor on FMU v2.
+ *
+ * @group Battery Calibration
+ */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
#else
-/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
-/* PX4IOAR: 0.00838095238 */
-/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
-/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+/**
+ * Scaling factor for battery voltage sensor on FMU v1.
+ *
+ * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659
+ * FMUv1 with PX4IO: 0.00459340659
+ * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238
+ *
+ * @group Battery Calibration
+ */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
+
+/**
+ * Scaling factor for battery current sensor.
+ *
+ * @group Battery Calibration
+ */
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
+
/**
* Roll control channel mapping.
*
@@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
+
+/**
+ * Return switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
+
+/**
+ * Assist switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+
+/**
+ * Mission switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+/**
+ * Flaps channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */
+/**
+ * Auxiliary switch 1 channel mapping.
+ *
+ * Default function: Camera pitch
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);
+
+/**
+ * Auxiliary switch 2 channel mapping.
+ *
+ * Default function: Camera roll
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */
+/**
+ * Auxiliary switch 3 channel mapping.
+ *
+ * Default function: Camera azimuth / yaw
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
+
+
+/**
+ * Roll scaling factor
+ *
+ * @group Radio Calibration
+ */
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
+
+/**
+ * Pitch scaling factor
+ *
+ * @group Radio Calibration
+ */
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
+
+/**
+ * Yaw scaling factor
+ *
+ * @group Radio Calibration
+ */
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
-PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */
+
+/**
+ * Failsafe channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FS_CH, 0);
+
+/**
+ * Failsafe channel mode.
+ *
+ * 0 = too low means signal loss,
+ * 1 = too high means signal loss
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FS_MODE, 0);
+
+/**
+ * Failsafe channel PWM threshold.
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 46eb7cc85..dc5bd95c6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -211,8 +211,8 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
- math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
- math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
+ math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
+ math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
@@ -470,8 +470,6 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
- _board_rotation(3, 3),
- _external_mag_rotation(3, 3),
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
@@ -639,41 +637,43 @@ Sensors::parameters_update()
if (!rc_valid)
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
+ const char *paramerr = "FAIL PARM LOAD";
+
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
- warnx("Failed getting roll chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
- warnx("Failed getting pitch chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
- warnx("Failed getting yaw chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
- warnx("Failed getting throttle chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
- warnx("Failed getting mode sw chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
- warnx("Failed getting return sw chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
- warnx("Failed getting assisted sw chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
- warnx("Failed getting mission sw chan index");
+ warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
- warnx("Failed getting flaps chan index");
+ warnx(paramerr);
}
// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
@@ -745,12 +745,12 @@ Sensors::parameters_update()
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
- warnx("Failed updating voltage scaling param");
+ warnx(paramerr);
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
- warnx("Failed updating current scaling param");
+ warnx(paramerr);
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
- math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+ math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer_m_s2[0] = vect(0);
@@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
- math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+ math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro_rad_s[0] = vect(0);
@@ -983,7 +983,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
- math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+ math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
if (_mag_is_external)
vect = _external_mag_rotation * vect;
@@ -1416,24 +1416,26 @@ Sensors::rc_poll()
}
}
+ /* mode switch input */
if (_rc.function[MODE] >= 0) {
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
+ /* assisted switch input */
+ if (_rc.function[ASSISTED] >= 0) {
+ manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+ }
+
+ /* mission switch input */
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
- /* land switch input */
+ /* return switch input */
if (_rc.function[RETURN] >= 0) {
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
}
- /* assisted switch input */
- if (_rc.function[ASSISTED] >= 0) {
- manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
- }
-
// if (_rc.function[OFFBOARD_MODE] >= 0) {
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
// }
@@ -1648,17 +1650,17 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (sensors::g_sensors != nullptr)
- errx(0, "sensors task already running");
+ errx(0, "already running");
sensors::g_sensors = new Sensors;
if (sensors::g_sensors == nullptr)
- errx(1, "sensors task alloc failed");
+ errx(1, "alloc failed");
if (OK != sensors::g_sensors->start()) {
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
- err(1, "sensors task start failed");
+ err(1, "start failed");
}
exit(0);
@@ -1666,7 +1668,7 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (sensors::g_sensors == nullptr)
- errx(1, "sensors task not running");
+ errx(1, "not running");
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
@@ -1675,10 +1677,10 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (sensors::g_sensors) {
- errx(0, "task is running");
+ errx(0, "is running");
} else {
- errx(1, "task is not running");
+ errx(1, "not running");
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-04 09:50:39 +0000