aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sensors/sensors.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-rw-r--r--src/modules/sensors/sensors.cpp75
1 files changed, 46 insertions, 29 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 259361be6..4fbc0416e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -145,7 +145,7 @@
#endif
static const int ERROR = -1;
-#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
+#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
/**
* Sensor app start / stop handling function
@@ -246,6 +246,7 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
struct rc_parameter_map_s _rc_parameter_map;
+ float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -523,6 +524,7 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+ _param_rc_values{},
_board_rotation{},
_mag_rotation{},
@@ -620,6 +622,18 @@ Sensors::Sensors() :
/* Barometer QNH */
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
+ // These are parameters for which QGroundControl always expects to be returned in a list request.
+ // We do a param_find here to force them into the list.
+ (void)param_find("RC_CHAN_CNT");
+ (void)param_find("RC_TH_USER");
+ (void)param_find("CAL_MAG0_ID");
+ (void)param_find("CAL_MAG1_ID");
+ (void)param_find("CAL_MAG2_ID");
+ (void)param_find("CAL_MAG0_ROT");
+ (void)param_find("CAL_MAG1_ROT");
+ (void)param_find("CAL_MAG2_ROT");
+ (void)param_find("SYS_PARAM_VER");
+
/* fetch initial parameter values */
parameters_update();
}
@@ -1382,14 +1396,13 @@ Sensors::parameter_update_poll(bool forced)
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
- warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
+ warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
/* apply new scaling and offsets */
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
if (res) {
- warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
+ warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
- gyro_count++;
config_ok = true;
}
}
@@ -1397,11 +1410,11 @@ Sensors::parameter_update_poll(bool forced)
}
}
- close(fd);
-
- if (!config_ok) {
- warnx("NO CONFIG FOR GYRO #%u", s);
+ if (config_ok) {
+ gyro_count++;
}
+
+ close(fd);
}
/* run through all accel sensors */
@@ -1449,14 +1462,13 @@ Sensors::parameter_update_poll(bool forced)
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
- warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
+ warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
/* apply new scaling and offsets */
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
if (res) {
- warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
+ warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
- accel_count++;
config_ok = true;
}
}
@@ -1464,11 +1476,11 @@ Sensors::parameter_update_poll(bool forced)
}
}
- close(fd);
-
- if (!config_ok) {
- warnx("NO CONFIG FOR ACCEL #%u", s);
+ if (config_ok) {
+ accel_count++;
}
+
+ close(fd);
}
/* run through all mag sensors */
@@ -1480,9 +1492,16 @@ Sensors::parameter_update_poll(bool forced)
int fd = open(str, 0);
if (fd < 0) {
+ /* the driver is not running, abort */
continue;
}
+ /* set a valid default rotation (same as board).
+ * if the mag is configured, this might be replaced
+ * in the section below.
+ */
+ _mag_rotation[s] = _board_rotation;
+
bool config_ok = false;
/* run through all stored calibrations */
@@ -1566,14 +1585,13 @@ Sensors::parameter_update_poll(bool forced)
}
if (failed) {
- warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
+ warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
if (res) {
- warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
+ warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
- mag_count++;
config_ok = true;
}
}
@@ -1581,11 +1599,11 @@ Sensors::parameter_update_poll(bool forced)
}
}
- close(fd);
-
- if (!config_ok) {
- warnx("NO CONFIG FOR MAG #%u", s);
+ if (config_ok) {
+ mag_count++;
}
+
+ close(fd);
}
int fd = open(AIRSPEED0_DEVICE_PATH, 0);
@@ -1605,7 +1623,8 @@ Sensors::parameter_update_poll(bool forced)
close(fd);
}
- warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
+ /* do not output this for now, as its covered in preflight checks */
+ // warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
}
}
@@ -1629,7 +1648,7 @@ 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]);
+ _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
} else {
_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
@@ -1834,8 +1853,6 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
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)
@@ -1847,8 +1864,8 @@ Sensors::set_params_from_rc()
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;
+ 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]);
@@ -2252,7 +2269,7 @@ Sensors::start()
int sensors_main(int argc, char *argv[])
{
- if (argc < 1) {
+ if (argc < 2) {
errx(1, "usage: sensors {start|stop|status}");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-11 05:38:09 +0000