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.cpp285
1 files changed, 140 insertions, 145 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 630c54335..87d7da537 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -184,13 +184,13 @@ private:
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
*/
- float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+ float get_rc_value(uint8_t func, float min_value, float max_value);
/**
* Get switch position for specified function.
*/
- switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
- switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
+ switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+ switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
/**
* Update paramters from RC channels if the functionality is activated and the
@@ -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);
@@ -813,28 +815,28 @@ Sensors::parameters_update()
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
/* update RC function mappings */
- _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
- _rc.function[ROLL] = _parameters.rc_map_roll - 1;
- _rc.function[PITCH] = _parameters.rc_map_pitch - 1;
- _rc.function[YAW] = _parameters.rc_map_yaw - 1;
-
- _rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
- _rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
- _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
- _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
- _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
- _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
-
- _rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
-
- _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
- _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
- _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
- _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
- _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
+
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
+
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
- _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
/* gyro offsets */
@@ -883,16 +885,19 @@ 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
+ /* set the accel internal sampling rate to default rate */
+ ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-
- /* 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
-
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-
- /* 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);
+ /* set the gyro internal sampling rate to default rate */
+ ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
-#endif
+ /* set the driver to poll at default rate */
+ ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
- 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,9 +1473,10 @@ 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[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[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)
* or no request to map this channel to a param has been sent via mavlink
*/
@@ -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 */
@@ -1646,7 +1628,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
float
-Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
{
if (_rc.function[func] >= 0) {
float value = _rc.channels[_rc.function[func]];
@@ -1667,41 +1649,41 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
}
switch_pos_t
-Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
+Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
- return SWITCH_POS_ON;
+ return manual_control_setpoint_s::SWITCH_POS_ON;
} else if (mid_inv ? value < mid_th : value > mid_th) {
- return SWITCH_POS_MIDDLE;
+ return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
} else {
- return SWITCH_POS_OFF;
+ return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
- return SWITCH_POS_NONE;
+ return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
switch_pos_t
-Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
- return SWITCH_POS_ON;
+ return manual_control_setpoint_s::SWITCH_POS_ON;
} else {
- return SWITCH_POS_OFF;
+ return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
- return SWITCH_POS_NONE;
+ return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
@@ -1709,22 +1691,23 @@ 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[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[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)
* or no request to map this channel to a param has been sent via mavlink
*/
continue;
}
- float rc_val = get_rc_value((enum 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 */
@@ -1847,24 +1832,24 @@ Sensors::rc_poll()
manual.timestamp = rc_input.timestamp_last_signal;
/* limit controls */
- manual.y = get_rc_value(ROLL, -1.0, 1.0);
- manual.x = get_rc_value(PITCH, -1.0, 1.0);
- manual.r = get_rc_value(YAW, -1.0, 1.0);
- manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
- manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
- manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
- manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
- manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
- manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
- manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
+ manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
+ manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
+ manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
+ manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
+ manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
+ manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
+ manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
+ manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
+ manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
+ manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* mode switches */
- manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
- manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
- manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
- manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
- manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
- manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+ manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+ manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+ manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+ manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+ manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+ manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {
@@ -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);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-01 12:41:20 +0000