diff --git a/.clang-tidy b/.clang-tidy index cc6c3dcf2c..fcd1faa706 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -151,7 +151,6 @@ Checks: '*, -readability-make-member-function-const, -bugprone-assignment-in-if-condition, -bugprone-implicit-widening-of-multiplication-result, - -bugprone-incorrect-roundings, -bugprone-macro-parentheses, -bugprone-multi-level-implicit-pointer-conversion, -bugprone-signed-char-misuse, diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 7e04629915..b6e508fc2a 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -40,6 +40,8 @@ #include "CollisionPrevention.hpp" #include "ObstacleMath.hpp" #include +#include + using namespace matrix; @@ -329,7 +331,7 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_ //3. this sensor data is out of range, the last reading was as well and this is the sensor with longest range //4. this sensor data is out of range, the last reading was valid and coming from the same sensor - uint16_t sensor_range_cm = static_cast(100.0f * sensor_range + 0.5f); //convert to cm + uint16_t sensor_range_cm = static_cast(lroundf(100.0f * sensor_range)); //convert to cm if (sensor_reading < sensor_range) { if ((_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index] @@ -406,13 +408,13 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude); } - uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm + uint16_t sensor_range = static_cast(lroundf(100.0f * distance_sensor.max_distance)); // convert to cm for (int bin = lower_bound; bin <= upper_bound; ++bin) { int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { - _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); + _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(lroundf(100.0f * distance_reading)); _data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp; _data_maxranges[wrapped_bin] = sensor_range; _data_fov[wrapped_bin] = 1; diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp index 8c34767eb5..be9762fa01 100644 --- a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -35,6 +35,7 @@ #include "FunctionProviderBase.hpp" +#include #include /** @@ -58,7 +59,7 @@ public: while (_topic.update(&vehicle_command)) { if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) { - int index = (int)(vehicle_command.param7 + 0.5f); + int index = static_cast(lroundf(vehicle_command.param7)); if (index == 0) { if (PX4_ISFINITE(vehicle_command.param1)) { _data[0] = vehicle_command.param1; } diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp index 8dfee6bb57..de63cd7f03 100644 --- a/src/lib/rc/sbus.cpp +++ b/src/lib/rc/sbus.cpp @@ -237,7 +237,7 @@ sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) * currently ignoring single bit channels. */ for (unsigned i = 0; (i < num_values) && (i < 16); ++i) { - value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); + value = static_cast(lround((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR)); /*protect from out of bounds values and limit to 11 bits*/ if (value > 0x07ff) { @@ -643,7 +643,7 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + values[channel] = static_cast(lround(value * SBUS_SCALE_FACTOR) + SBUS_SCALE_OFFSET); } /* decode switch channels if data fields are wide enough */ diff --git a/src/lib/rc/st24.cpp b/src/lib/rc/st24.cpp index 3e0b2f6851..69b9737c91 100644 --- a/src/lib/rc/st24.cpp +++ b/src/lib/rc/st24.cpp @@ -41,6 +41,7 @@ #include #include +#include #include "st24.h" #include "common_rc.h" @@ -180,13 +181,13 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan channels[chan_index] = ((uint16_t)d->channel[i] << 4); channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + channels[chan_index] = (uint16_t)lroundf(channels[chan_index] * ST24_SCALE_FACTOR) + ST24_SCALE_OFFSET; chan_index++; channels[chan_index] = ((uint16_t)d->channel[i + 2]); channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + channels[chan_index] = (uint16_t)lroundf(channels[chan_index] * ST24_SCALE_FACTOR) + ST24_SCALE_OFFSET; chan_index++; } } @@ -209,13 +210,13 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan channels[chan_index] = ((uint16_t)d->channel[i] << 4); channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + channels[chan_index] = static_cast(lroundf(channels[chan_index] * ST24_SCALE_FACTOR) + ST24_SCALE_OFFSET); chan_index++; channels[chan_index] = ((uint16_t)d->channel[i + 2]); channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + channels[chan_index] = static_cast(lroundf(channels[chan_index] * ST24_SCALE_FACTOR) + ST24_SCALE_OFFSET); chan_index++; } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0ba1619a51..38d9086f26 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -981,7 +981,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_SET_NAV_STATE: { // Used from ROS - uint8_t desired_nav_state = (uint8_t)(cmd.param1 + 0.5f); + uint8_t desired_nav_state = static_cast(lroundf(cmd.param1)); if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1676,7 +1676,7 @@ unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd) actuator_test_s actuator_test{}; actuator_test.timestamp = hrt_absolute_time(); - actuator_test.function = (int)(cmd.param5 + 0.5); + actuator_test.function = static_cast(lroundf(cmd.param5)); if (actuator_test.function < 1000) { const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION @@ -1701,7 +1701,7 @@ unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd) actuator_test.value = cmd.param1; actuator_test.action = actuator_test_s::ACTION_DO_CONTROL; - int timeout_ms = (int)(cmd.param2 * 1000.f + 0.5f); + int timeout_ms = static_cast(lroundf(cmd.param2 * 1000.f)); if (timeout_ms <= 0) { actuator_test.action = actuator_test_s::ACTION_RELEASE_CONTROL; diff --git a/src/modules/commander/failure_detector/FailureInjector.cpp b/src/modules/commander/failure_detector/FailureInjector.cpp index e1633500c3..9bbab47342 100644 --- a/src/modules/commander/failure_detector/FailureInjector.cpp +++ b/src/modules/commander/failure_detector/FailureInjector.cpp @@ -33,6 +33,7 @@ #include "FailureInjector.hpp" +#include #include #include @@ -53,15 +54,15 @@ void FailureInjector::update() vehicle_command_s vehicle_command; while (_vehicle_command_sub.update(&vehicle_command)) { - const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); - const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + const int failure_unit = static_cast(lroundf(vehicle_command.param1)); + const int failure_type = static_cast(lroundf(vehicle_command.param2)); if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE || failure_unit != vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) { continue; } - const int instance = static_cast(vehicle_command.param3 + 0.5f); + const int instance = static_cast(lroundf(vehicle_command.param3)); bool supported = false; for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index e9691600a0..f83322511f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -33,6 +33,7 @@ #include "FlightModeManager.hpp" +#include #include #include @@ -324,7 +325,7 @@ void FlightModeManager::handleCommand() if (_current_task.task) { // check for other commands not related to task switching if ((command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) - && (static_cast(command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_GROUNDSPEED) + && (static_cast(lroundf(command.param1)) == vehicle_command_s::SPEED_TYPE_GROUNDSPEED) && (command.param2 > 0.f)) { _current_task.task->overrideCruiseSpeed(command.param2); } diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 55c6b64273..ad052bd6f7 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -90,7 +90,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b // commanded heading behaviour if (PX4_ISFINITE(command.param3)) { - if (static_cast(command.param3 + .5f) == vehicle_command_s::ORBIT_YAW_BEHAVIOUR_UNCHANGED) { + if (static_cast(lround(command.param3)) == vehicle_command_s::ORBIT_YAW_BEHAVIOUR_UNCHANGED) { if (!_currently_orbiting) { // only change the yaw behaviour if we are not actively orbiting _yaw_behaviour = _param_mc_orbit_yaw_mod.get(); } diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 9cbcd2ea4c..66cbcc18d0 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -33,6 +33,7 @@ #include "FixedWingModeManager.hpp" +#include #include #include @@ -139,7 +140,7 @@ FixedWingModeManager::vehicle_command_poll() } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { - if ((static_cast(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) { + if ((static_cast(lroundf(vehicle_command.param1)) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) { if (vehicle_command.param2 > FLT_EPSILON) { // param2 is an equivalent airspeed setpoint if (_control_mode_current == FW_POSCTRL_MODE_AUTO) { _pos_sp_triplet.current.cruising_speed = vehicle_command.param2; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index de7ea22155..1203d25935 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -323,9 +323,9 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ // Stabilization params are ignored. Use MNT_DO_STAB param instead. const int params[] = { - (int)((float) vehicle_command.param5 + 0.5f), - (int)((float) vehicle_command.param6 + 0.5f), - (int)(vehicle_command.param7 + 0.5f) + static_cast(lroundf(vehicle_command.param5)), + static_cast(lroundf(vehicle_command.param6)), + static_cast(lroundf(vehicle_command.param7)) }; for (int i = 0; i < 3; ++i) { @@ -779,9 +779,9 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ // Stabilization params are ignored. Use MNT_DO_STAB param instead. const int params[] = { - (int)((float) vehicle_command.param5 + 0.5f), - (int)((float) vehicle_command.param6 + 0.5f), - (int)(vehicle_command.param7 + 0.5f) + static_cast(lroundf(vehicle_command.param5)), + static_cast(lroundf(vehicle_command.param6)), + static_cast(lroundf(vehicle_command.param7)) }; for (int i = 0; i < 3; ++i) { diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 948e35c0e9..b0c306fa55 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -1185,7 +1186,7 @@ void Logger::handle_vehicle_command_update() if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { - if ((int)(command.param1 + 0.5f) != 0) { + if (static_cast(lroundf(command.param1)) != 0) { ack_vehicle_command(&command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } else if (can_start_mavlink_log()) { diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 79fa7aefd9..c1262d42b2 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -1375,7 +1375,7 @@ bool MissionBase::haveCachedCameraModeItems() bool MissionBase::cameraWasTriggering() { return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL - && (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) || + && static_cast(lround(_last_camera_trigger_item.params[0])) == 1) || (_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST && _last_camera_trigger_item.params[0] > FLT_EPSILON); diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 5b37bafa57..4f2957cfea 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "BatterySimulator.hpp" +#include ModuleBase::Descriptor BatterySimulator::desc{task_spawn, custom_command, print_usage}; @@ -130,9 +131,9 @@ void BatterySimulator::updateCommands() bool handled = false; bool supported = false; - const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); - const int failure_type = static_cast(vehicle_command.param2 + 0.5f); - const int instance = static_cast(vehicle_command.param3 + 0.5f); + const int failure_unit = static_cast(lroundf(vehicle_command.param1)); + const int failure_type = static_cast(lroundf(vehicle_command.param2)); + const int instance = static_cast(lroundf(vehicle_command.param3)); if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_BATTERY) { diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 91ec04ac67..dec6a60b7e 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -1255,9 +1255,9 @@ void SimulatorMavlink::check_failure_injections() bool handled = false; bool supported = false; - const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); - const int failure_type = static_cast(vehicle_command.param2 + 0.5f); - const int instance = static_cast(vehicle_command.param3 + 0.5f); + const int failure_unit = static_cast(std::lround(vehicle_command.param1)); + const int failure_type = static_cast(std::lround(vehicle_command.param2)); + const int instance = static_cast(std::lround(vehicle_command.param3)); if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { handled = true; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0047fe1c4f..43f37b3c60 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -149,7 +149,7 @@ void VtolAttitudeControl::vehicle_cmd_poll() uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - const int transition_command_param1 = int(vehicle_command.param1 + 0.5f); + const int transition_command_param1 = static_cast(lround(vehicle_command.param1)); // deny transition from MC to FW in Takeoff, Land, RTL and Orbit if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW && @@ -162,7 +162,7 @@ void VtolAttitudeControl::vehicle_cmd_poll() } else { _transition_command = transition_command_param1; - _immediate_transition = (PX4_ISFINITE(vehicle_command.param2)) ? int(vehicle_command.param2 + 0.5f) : false; + _immediate_transition = (PX4_ISFINITE(vehicle_command.param2)) ? static_cast(lround(vehicle_command.param2)) : false; // reset fixed_wing_system_failure flag when a new transition to FW is triggered if (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {