Skip to content

Commit

Permalink
voted_sensors_update: refactor to camelCase function names
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR
MaEtUgR committed Jun 27, 2019
1 parent a4a6593 commit e3c75c0
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 70 deletions.
26 changes: 13 additions & 13 deletions src/modules/sensors/sensors.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,7 +270,7 @@ Sensors::parameters_update()
}

_rc_update.update_rc_functions();
_voted_sensors_update.parameters_update();
_voted_sensors_update.parametersUpdate();

return ret;
}
Expand Down Expand Up @@ -567,7 +567,7 @@ Sensors::run()
parameter_update_poll(true);

/* get a set of initial values */
_voted_sensors_update.sensors_poll(raw, airdata, magnetometer);
_voted_sensors_update.sensorsPoll(raw, airdata, magnetometer);

diff_pres_poll(airdata);

Expand All @@ -584,7 +584,7 @@ Sensors::run()
while (!should_exit()) {

/* use the best-voted gyro to pace output */
poll_fds.fd = _voted_sensors_update.best_gyro_fd();
poll_fds.fd = _voted_sensors_update.bestGyroFd();

/* wait for up to 50ms for data (Note that this implies, we can have a fail-over time of 50ms,
* if a gyro fails) */
Expand All @@ -598,8 +598,8 @@ Sensors::run()
/* if the polling operation failed because no gyro sensor is available yet,
* then attempt to subscribe once again
*/
if (_voted_sensors_update.num_gyros() == 0) {
_voted_sensors_update.initialize_sensors();
if (_voted_sensors_update.numGyros() == 0) {
_voted_sensors_update.initializeSensors();
}

px4_usleep(1000);
Expand All @@ -615,12 +615,12 @@ Sensors::run()
_armed = vcontrol_mode.flag_armed;
}

/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
/* the timestamp of the raw struct is updated by the gyroPoll() method (this makes the gyro
* a mandatory sensor) */
const uint64_t airdata_prev_timestamp = airdata.timestamp;
const uint64_t magnetometer_prev_timestamp = magnetometer.timestamp;

_voted_sensors_update.sensors_poll(raw, airdata, magnetometer);
_voted_sensors_update.sensorsPoll(raw, airdata, magnetometer);

/* check battery voltage */
adc_poll();
Expand All @@ -629,7 +629,7 @@ Sensors::run()

if (raw.timestamp > 0) {

_voted_sensors_update.set_relative_timestamps(raw);
_voted_sensors_update.setRelativeTimestamps(raw);

int instance;
orb_publish_auto(ORB_ID(sensor_combined), &_sensor_pub, &raw, &instance, ORB_PRIO_DEFAULT);
Expand All @@ -642,15 +642,15 @@ Sensors::run()
orb_publish_auto(ORB_ID(vehicle_magnetometer), &_magnetometer_pub, &magnetometer, &instance, ORB_PRIO_DEFAULT);
}

_voted_sensors_update.check_failover();
_voted_sensors_update.checkFailover();

/* If the the vehicle is disarmed calculate the length of the maximum difference between
* IMU units as a consistency metric and publish to the sensor preflight topic
*/
if (!_armed) {
preflt.timestamp = hrt_absolute_time();
_voted_sensors_update.calc_accel_inconsistency(preflt);
_voted_sensors_update.calc_gyro_inconsistency(preflt);
_voted_sensors_update.calcAccelInconsistency(preflt);
_voted_sensors_update.calcGyroInconsistency(preflt);
_voted_sensors_update.calcMagInconsistency(preflt);
orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt);
}
Expand All @@ -660,7 +660,7 @@ Sensors::run()
* when not adding sensors poll for param updates
*/
if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) {
_voted_sensors_update.initialize_sensors();
_voted_sensors_update.initializeSensors();
last_config_update = hrt_absolute_time();

} else {
Expand Down Expand Up @@ -713,7 +713,7 @@ int Sensors::task_spawn(int argc, char *argv[])

int Sensors::print_status()
{
_voted_sensors_update.print_status();
_voted_sensors_update.printStatus();

PX4_INFO("Airspeed status:");
_airspeed_validator.print();
Expand Down
68 changes: 34 additions & 34 deletions src/modules/sensors/voted_sensors_update.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,20 +95,20 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.timestamp = 0;

initialize_sensors();
initializeSensors();

_corrections_changed = true; //make sure to initially publish the corrections topic
_selection_changed = true;

return 0;
}

void VotedSensorsUpdate::initialize_sensors()
void VotedSensorsUpdate::initializeSensors()
{
init_sensor_class(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
init_sensor_class(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
init_sensor_class(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
init_sensor_class(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
}

void VotedSensorsUpdate::deinit()
Expand All @@ -130,7 +130,7 @@ void VotedSensorsUpdate::deinit()
}
}

void VotedSensorsUpdate::parameters_update()
void VotedSensorsUpdate::parametersUpdate()
{
/* fine tune board offset */
Dcmf board_rotation_offset = Eulerf(
Expand Down Expand Up @@ -279,7 +279,7 @@ void VotedSensorsUpdate::parameters_update()

} else {
/* apply new scaling and offsets */
config_ok = apply_gyro_calibration(h, &gscale, device_id);
config_ok = applyGyroCalibration(h, &gscale, device_id);

if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
Expand Down Expand Up @@ -367,7 +367,7 @@ void VotedSensorsUpdate::parameters_update()

} else {
/* apply new scaling and offsets */
config_ok = apply_accel_calibration(h, &ascale, device_id);
config_ok = applyAccelCalibration(h, &ascale, device_id);

if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
Expand Down Expand Up @@ -512,7 +512,7 @@ void VotedSensorsUpdate::parameters_update()
} else {

/* apply new scaling and offsets */
config_ok = apply_mag_calibration(h, &mscale, device_id);
config_ok = applyMagCalibration(h, &mscale, device_id);

if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
Expand All @@ -526,7 +526,7 @@ void VotedSensorsUpdate::parameters_update()

}

void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
{
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 };
float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 };
Expand Down Expand Up @@ -633,7 +633,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
}
}

void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
{
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 };
float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 };
Expand Down Expand Up @@ -741,7 +741,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
}
}

void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer)
void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
{
for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) {
bool mag_updated;
Expand Down Expand Up @@ -793,7 +793,7 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer)
}
}

void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata)
void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
{
bool got_update = false;
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
Expand Down Expand Up @@ -892,7 +892,7 @@ void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata)
}
}

bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_name, const uint64_t type)
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
{
if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {

Expand Down Expand Up @@ -966,7 +966,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
return false;
}

void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data,
void VotedSensorsUpdate::initSensorClass(const struct orb_metadata *meta, SensorData &sensor_data,
uint8_t sensor_count_max)
{
int max_sensor_index = -1;
Expand Down Expand Up @@ -996,7 +996,7 @@ void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, Sens
}
}

void VotedSensorsUpdate::print_status()
void VotedSensorsUpdate::printStatus()
{
PX4_INFO("gyro status:");
_gyro.voter.print();
Expand All @@ -1011,7 +1011,7 @@ void VotedSensorsUpdate::print_status()
}

bool
VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
VotedSensorsUpdate::applyGyroCalibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
{
#if defined(__PX4_NUTTX)

Expand All @@ -1025,7 +1025,7 @@ VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const struct gyro_calib
}

bool
VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
VotedSensorsUpdate::applyAccelCalibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
{
#if defined(__PX4_NUTTX)

Expand All @@ -1039,7 +1039,7 @@ VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_cal
}

bool
VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
VotedSensorsUpdate::applyMagCalibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
{
#if defined(__PX4_NUTTX)

Expand All @@ -1056,13 +1056,13 @@ VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibra
#endif
}

void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s &airdata,
vehicle_magnetometer_s &magnetometer)
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata,
vehicle_magnetometer_s &magnetometer)
{
accel_poll(raw);
gyro_poll(raw);
mag_poll(magnetometer);
baro_poll(airdata);
accelPoll(raw);
gyroPoll(raw);
magPoll(magnetometer);
baroPoll(airdata);

// publish sensor corrections if necessary
if (_corrections_changed) {
Expand Down Expand Up @@ -1093,15 +1093,15 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s
}
}

void VotedSensorsUpdate::check_failover()
void VotedSensorsUpdate::checkFailover()
{
check_failover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
check_failover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
check_failover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
check_failover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
checkFailover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
}

void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw)
void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
{
if (_last_accel_timestamp[_accel.last_best_vote]) {
raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] -
Expand All @@ -1110,7 +1110,7 @@ void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw)
}

void
VotedSensorsUpdate::calc_accel_inconsistency(sensor_preflight_s &preflt)
VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
{
float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
unsigned check_index = 0; // the number of sensors the primary has been checked against
Expand Down Expand Up @@ -1159,7 +1159,7 @@ VotedSensorsUpdate::calc_accel_inconsistency(sensor_preflight_s &preflt)
}
}

void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt)
void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
{
float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
unsigned check_index = 0; // the number of sensors the primary has been checked against
Expand Down
Loading

0 comments on commit e3c75c0

Please sign in to comment.