From ba00476e9c0beee69b6abf52080c5a3d2f98af8f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 27 Jun 2019 14:58:14 +0100 Subject: [PATCH] voted_sensors_update: refactor to camelCase function names --- src/modules/sensors/sensors.cpp | 26 ++++---- src/modules/sensors/voted_sensors_update.cpp | 68 ++++++++++---------- src/modules/sensors/voted_sensors_update.h | 46 ++++++------- 3 files changed, 70 insertions(+), 70 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c1d816cfd3b3..a9cfa325755f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -270,7 +270,7 @@ Sensors::parameters_update() } _rc_update.update_rc_functions(); - _voted_sensors_update.parameters_update(); + _voted_sensors_update.parametersUpdate(); return ret; } @@ -573,7 +573,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); @@ -590,7 +590,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) */ @@ -604,8 +604,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); @@ -621,12 +621,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(); @@ -635,7 +635,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); @@ -648,15 +648,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); } @@ -666,7 +666,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 { @@ -719,7 +719,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(); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index ea710e8bf5a8..e3e0e3b3fa11 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -95,7 +95,7 @@ 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; @@ -103,12 +103,12 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) 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() @@ -130,7 +130,7 @@ void VotedSensorsUpdate::deinit() } } -void VotedSensorsUpdate::parameters_update() +void VotedSensorsUpdate::parametersUpdate() { /* fine tune board offset */ Dcmf board_rotation_offset = Eulerf( @@ -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); @@ -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); @@ -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); @@ -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 }; @@ -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 }; @@ -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; @@ -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 }; @@ -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) { @@ -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; @@ -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(); @@ -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) @@ -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) @@ -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) @@ -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) { @@ -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] - @@ -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 @@ -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 diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 434d76aba41d..cf1fe3ea7c43 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -91,51 +91,51 @@ class VotedSensorsUpdate /** * This tries to find new sensor instances. This is called from init(), then it can be called periodically. */ - void initialize_sensors(); + void initializeSensors(); /** * deinitialize the object (we cannot use the destructor because it is called on the wrong thread) */ void deinit(); - void print_status(); + void printStatus(); /** - * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least + * call this whenever parameters got updated. Make sure to have initializeSensors() called at least * once before calling this. */ - void parameters_update(); + void parametersUpdate(); /** * read new sensor data */ - void sensors_poll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer); + void sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer); /** - * set the relative timestamps of each sensor timestamp, based on the last sensors_poll, + * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, * so that the data can be published. */ - void set_relative_timestamps(sensor_combined_s &raw); + void setRelativeTimestamps(sensor_combined_s &raw); /** * check if a failover event occured. if so, report it. */ - void check_failover(); + void checkFailover(); - int num_gyros() const { return _gyro.subscription_count; } - int gyro_fd(int idx) const { return _gyro.subscription[idx]; } + int numGyros() const { return _gyro.subscription_count; } + int gyroFd(int idx) const { return _gyro.subscription[idx]; } - int best_gyro_fd() const { return _gyro.subscription[_gyro.last_best_vote]; } + int bestGyroFd() const { return _gyro.subscription[_gyro.last_best_vote]; } /** * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor */ - void calc_accel_inconsistency(sensor_preflight_s &preflt); + void calcAccelInconsistency(sensor_preflight_s &preflt); /** * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor */ - void calc_gyro_inconsistency(sensor_preflight_s &preflt); + void calcGyroInconsistency(sensor_preflight_s &preflt); /** * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers @@ -168,7 +168,7 @@ class VotedSensorsUpdate unsigned int last_failover_count; }; - void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max); + void initSensorClass(const struct orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max); /** * Poll the accelerometer for updated data. @@ -176,7 +176,7 @@ class VotedSensorsUpdate * @param raw Combined sensor data structure into which * data should be returned. */ - void accel_poll(struct sensor_combined_s &raw); + void accelPoll(struct sensor_combined_s &raw); /** * Poll the gyro for updated data. @@ -184,7 +184,7 @@ class VotedSensorsUpdate * @param raw Combined sensor data structure into which * data should be returned. */ - void gyro_poll(struct sensor_combined_s &raw); + void gyroPoll(struct sensor_combined_s &raw); /** * Poll the magnetometer for updated data. @@ -192,7 +192,7 @@ class VotedSensorsUpdate * @param raw Combined sensor data structure into which * data should be returned. */ - void mag_poll(vehicle_magnetometer_s &magnetometer); + void magPoll(vehicle_magnetometer_s &magnetometer); /** * Poll the barometer for updated data. @@ -200,13 +200,13 @@ class VotedSensorsUpdate * @param raw Combined sensor data structure into which * data should be returned. */ - void baro_poll(vehicle_air_data_s &airdata); + void baroPoll(vehicle_air_data_s &airdata); /** * Check & handle failover of a sensor * @return true if a switch occured (could be for a non-critical reason) */ - bool check_failover(SensorData &sensor, const char *sensor_name, const uint64_t type); + bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type); /** * Apply a gyro calibration. @@ -216,7 +216,7 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_gyro_calibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id); + bool applyGyroCalibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id); /** * Apply a accel calibration. @@ -226,8 +226,8 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_accel_calibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, - const int device_id); + bool applyAccelCalibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, + const int device_id); /** * Apply a mag calibration. @@ -237,7 +237,7 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_mag_calibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); + bool applyMagCalibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); SensorData _gyro; SensorData _accel;