fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 03:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors, commander: code style fixed

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-11 13:36:51 +02:00
parent 88b18bbad1
commit 349809f535
8 changed files with 344 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+70 -32
View File
@@ -220,8 +220,8 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
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 */
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 */
@@ -644,8 +644,9 @@ Sensors::parameters_update()
}
/* handle wrong values */
if (!rc_valid)
if (!rc_valid) {
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
}
const char *paramerr = "FAIL PARM LOAD";
@@ -701,19 +702,19 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
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);
_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
_parameters.rc_auto_inv = (_parameters.rc_auto_th<0);
_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
_parameters.rc_posctl_inv = (_parameters.rc_posctl_th<0);
_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
_parameters.rc_return_inv = (_parameters.rc_return_th<0);
_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
_parameters.rc_return_th = fabs(_parameters.rc_return_th);
param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
_parameters.rc_loiter_inv = (_parameters.rc_loiter_th<0);
_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
/* update RC function mappings */
@@ -843,12 +844,14 @@ Sensors::gyro_init()
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the gyro internal sampling rate up to at least 1000Hz */
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) {
ioctl(fd, GYROIOCSSAMPLERATE, 800);
}
/* set the driver to poll at 1000Hz */
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) {
ioctl(fd, SENSORIOCSPOLLRATE, 800);
}
#else
@@ -903,12 +906,15 @@ Sensors::mag_init()
ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
if (ret < 0)
if (ret < 0) {
errx(1, "FATAL: unknown if magnetometer is external or onboard");
else if (ret == 1)
} else if (ret == 1) {
_mag_is_external = true;
else
} else {
_mag_is_external = false;
}
close(fd);
}
@@ -1008,10 +1014,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
if (_mag_is_external)
if (_mag_is_external) {
vect = _external_mag_rotation * vect;
else
} else {
vect = _board_rotation * vect;
}
raw.magnetometer_ga[0] = vect(0);
raw.magnetometer_ga[1] = vect(1);
@@ -1129,8 +1137,9 @@ Sensors::parameter_update_poll(bool forced)
_parameters.gyro_scale[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) {
warn("WARNING: failed to set scale / offsets for gyro");
}
close(fd);
@@ -1144,8 +1153,9 @@ Sensors::parameter_update_poll(bool forced)
_parameters.accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) {
warn("WARNING: failed to set scale / offsets for accel");
}
close(fd);
@@ -1159,8 +1169,9 @@ Sensors::parameter_update_poll(bool forced)
_parameters.mag_scale[2],
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) {
warn("WARNING: failed to set scale / offsets for mag");
}
close(fd);
@@ -1174,8 +1185,10 @@ Sensors::parameter_update_poll(bool forced)
1.0f,
};
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
}
close(fd);
}
@@ -1193,10 +1206,12 @@ void
Sensors::adc_poll(struct sensor_combined_s &raw)
{
/* only read if publishing */
if (!_publishing)
if (!_publishing) {
return;
}
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (t - _last_adc >= 10000) {
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
@@ -1221,6 +1236,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
@@ -1239,19 +1255,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
if (_battery_status.discharged_mah < 0.0f) {
_battery_status.discharged_mah = 0.0f;
}
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1284,7 +1305,9 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
_last_adc = t;
if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
@@ -1303,6 +1326,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
{
if (_rc.function[func] >= 0) {
float value = _rc.chan[_rc.function[func]].scaled;
if (value < min_value) {
return min_value;
@@ -1312,6 +1336,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
} else {
return value;
}
} else {
return 0.0f;
}
@@ -1322,6 +1347,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
@@ -1342,6 +1368,7 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
@@ -1380,13 +1407,15 @@ Sensors::rc_poll()
/* check failsafe */
int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle
if (_parameters.rc_map_failsafe>0){ // if not 0, use channel number instead of rc.function mapping
if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping
fs_ch = _parameters.rc_map_failsafe - 1;
}
if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) {
/* failsafe configured */
if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) ||
(_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
(_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
/* failsafe triggered, signal is lost by receiver */
signal_lost = true;
}
@@ -1395,8 +1424,9 @@ Sensors::rc_poll()
unsigned channel_limit = rc_input.channel_count;
if (channel_limit > _rc_max_chan_count)
if (channel_limit > _rc_max_chan_count) {
channel_limit = _rc_max_chan_count;
}
/* read out and scale values from raw message even if signal is invalid */
for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1404,11 +1434,13 @@ Sensors::rc_poll()
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (rc_input.values[i] < _parameters.min[i])
if (rc_input.values[i] < _parameters.min[i]) {
rc_input.values[i] = _parameters.min[i];
}
if (rc_input.values[i] > _parameters.max[i])
if (rc_input.values[i] > _parameters.max[i]) {
rc_input.values[i] = _parameters.max[i];
}
/*
* 2) Scale around the mid point differently for lower and upper range.
@@ -1440,8 +1472,9 @@ Sensors::rc_poll()
_rc.chan[i].scaled *= _parameters.rev[i];
/* handle any parameter-induced blowups */
if (!isfinite(_rc.chan[i].scaled))
if (!isfinite(_rc.chan[i].scaled)) {
_rc.chan[i].scaled = 0.0f;
}
}
_rc.chan_count = rc_input.channel_count;
@@ -1623,8 +1656,9 @@ Sensors::task_main()
diff_pres_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing)
if (_publishing) {
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
}
/* Look for new r/c input data */
rc_poll();
@@ -1661,18 +1695,21 @@ Sensors::start()
int sensors_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
errx(1, "usage: sensors {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (sensors::g_sensors != nullptr)
if (sensors::g_sensors != nullptr) {
errx(0, "already running");
}
sensors::g_sensors = new Sensors;
if (sensors::g_sensors == nullptr)
if (sensors::g_sensors == nullptr) {
errx(1, "alloc failed");
}
if (OK != sensors::g_sensors->start()) {
delete sensors::g_sensors;
@@ -1684,8 +1721,9 @@ int sensors_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "stop")) {
if (sensors::g_sensors == nullptr)
if (sensors::g_sensors == nullptr) {
errx(1, "not running");
}
delete sensors::g_sensors;
sensors::g_sensors = nullptr;