diff --git a/cmake/px4_add_common_flags.cmake b/cmake/px4_add_common_flags.cmake index d180e92b85..6647a5e42b 100644 --- a/cmake/px4_add_common_flags.cmake +++ b/cmake/px4_add_common_flags.cmake @@ -85,6 +85,8 @@ function(px4_add_common_flags) -Wunknown-pragmas -Wunused-variable + -Wfloat-conversion + # disabled warnings -Wno-missing-field-initializers -Wno-missing-include-dirs # TODO: fix and enable diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/platforms/common/uORB/uORBDeviceMaster.cpp index caf28111b9..a790e45dfa 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/platforms/common/uORB/uORBDeviceMaster.cpp @@ -378,7 +378,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) while (cur_node) { unsigned int num_msgs = cur_node->node->updates_available(cur_node->last_pub_msg_count); - cur_node->pub_msg_delta = roundf(num_msgs / dt); + cur_node->pub_msg_delta = (int)roundf(num_msgs / dt); cur_node->last_pub_msg_count += num_msgs; total_size += cur_node->pub_msg_delta * cur_node->node->get_meta()->o_size; diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index b57882fbe9..e0e784e368 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -342,10 +342,11 @@ CameraTrigger::update_intervalometer() PX4_DEBUG("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused); // schedule trigger on and off calls - hrt_call_every(&_engagecall, 0, (_interval * 1000), &CameraTrigger::engage, this); + hrt_call_every(&_engagecall, 0, (int)(_interval * 1000), &CameraTrigger::engage, this); // schedule trigger on and off calls - hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), &CameraTrigger::disengage, this); + hrt_call_every(&_disengagecall, (int)(0 + (_activation_time * 1000)), (int)(_interval * 1000), + &CameraTrigger::disengage, this); } } @@ -429,7 +430,7 @@ CameraTrigger::shoot_once() hrt_call_after(&_engagecall, 0, (hrt_callout)&CameraTrigger::engage, this); - hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), &CameraTrigger::disengage, this); + hrt_call_after(&_disengagecall, (int)(0 + (_activation_time * 1000)), &CameraTrigger::disengage, this); } bool @@ -672,8 +673,8 @@ CameraTrigger::Run() if (cmd.param4 >= 2.0f) { _CAMPOS_num_poses = commandParamToInt(cmd.param4); - _CAMPOS_roll_angle = cmd.param5; - _CAMPOS_pitch_angle = cmd.param6; + _CAMPOS_roll_angle = static_cast(cmd.param5); + _CAMPOS_pitch_angle = static_cast(cmd.param6); _CAMPOS_angle_interval = _CAMPOS_roll_angle * 2 / (_CAMPOS_num_poses - 1); _CAMPOS_pose_counter = 0; _CAMPOS_updated_roll_angle = false; diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp index c6e7571492..3733c986e2 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp +++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp @@ -115,7 +115,7 @@ LightwareLaserSerial::init() /* SF30/B (50m 39Hz) */ _px4_rangefinder.set_min_distance(0.2f); _px4_rangefinder.set_max_distance(50.0f); - _interval = 1e6 / 39; + _interval = (int)round(1e6 / 39); _simple_serial = true; break; @@ -123,7 +123,7 @@ LightwareLaserSerial::init() /* SF30/C (100m 39Hz) */ _px4_rangefinder.set_min_distance(0.2f); _px4_rangefinder.set_max_distance(100.0f); - _interval = 1e6 / 39; + _interval = (int)round(1e6 / 39); _simple_serial = true; break; diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 96e517f355..745aeb32f9 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( MAIN gps COMPILE_FLAGS -Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707 + -Wno-float-conversion # TODO: fix and enable SRCS gps.cpp devices/src/gps_helper.cpp diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index cea6077264..6393555653 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -894,7 +894,7 @@ GPS::run() float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; _rate = last_rate_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; - _rate_reading = _num_bytes_read / dt; + _rate_reading = (int)roundf((float)_num_bytes_read / dt); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _last_rate_rtcm_injection_count = 0; diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 6253f6e4d3..54f3ac7a9d 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp @@ -63,7 +63,7 @@ int rpm_simulator_main(int argc, char *argv[]) uORB::Publication rpm_pub{ORB_ID(rpm)}; uint64_t timestamp_us = hrt_absolute_time(); - float frequency = atof(argv[1]); + float frequency = (float)atof(argv[1]); // prpepare RPM data message rpm.timestamp = timestamp_us; diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index 1605a2f9b9..4be833c037 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -38,9 +38,9 @@ using namespace time_literals; FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _latitude(latitude_deg * 10e6), - _longitude(longitude_deg * 10e6), - _altitude(altitude_m * 10e2f) + _latitude((int32_t)(latitude_deg * 10e6)), + _longitude((int32_t)(longitude_deg * 10e6)), + _altitude((int32_t)(altitude_m * 10e2f)) { } diff --git a/src/examples/fake_imu/FakeImu.cpp b/src/examples/fake_imu/FakeImu.cpp index d88a0ffb13..3df5d8d93e 100644 --- a/src/examples/fake_imu/FakeImu.cpp +++ b/src/examples/fake_imu/FakeImu.cpp @@ -41,7 +41,7 @@ FakeImu::FakeImu() : _px4_accel(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION _px4_gyro(1310988) // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION { - _sensor_interval_us = roundf(1.e6f / _px4_gyro.get_max_rate_hz()); + _sensor_interval_us = (uint32_t)roundf(1.e6f / _px4_gyro.get_max_rate_hz()); PX4_INFO("Rate %.3f, Interval: %" PRId32 " us", (double)_px4_gyro.get_max_rate_hz(), _sensor_interval_us); @@ -66,7 +66,7 @@ void FakeImu::Run() sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = hrt_absolute_time(); - gyro.samples = roundf(IMU_RATE_HZ / (1e6 / _sensor_interval_us)); + gyro.samples = (uint8_t)roundf(IMU_RATE_HZ / (1e6f / (float)_sensor_interval_us)); gyro.dt = 1e6 / IMU_RATE_HZ; const double dt_s = 1 / IMU_RATE_HZ; @@ -92,9 +92,9 @@ void FakeImu::Run() const double timestamp_sample_s = static_cast(gyro.timestamp_sample - _time_start_us) / 1e6; - float x_freq = 0; - float y_freq = 0; - float z_freq = 0; + double x_freq = 0; + double y_freq = 0; + double z_freq = 0; for (int n = 0; n < gyro.samples; n++) { // timestamp_sample corresponds to last sample @@ -105,9 +105,9 @@ void FakeImu::Run() const double y_F = y_f0 + (y_f1 - y_f0) * t / (2 * T); const double z_F = z_f0 + (z_f1 - z_f0) * t / (2 * T); - gyro.x[n] = roundf(A * sin(2 * M_PI * x_F * t)); - gyro.y[n] = roundf(A * sin(2 * M_PI * y_F * t)); - gyro.z[n] = roundf(A * sin(2 * M_PI * z_F * t)); + gyro.x[n] = (int16_t)round(A * sin(2 * M_PI * x_F * t)); + gyro.y[n] = (int16_t)round(A * sin(2 * M_PI * y_F * t)); + gyro.z[n] = (int16_t)round(A * sin(2 * M_PI * z_F * t)); if (n == 0) { x_freq = (x_f1 - x_f0) * (t / T) + x_f0; diff --git a/src/examples/fake_imu/FakeImu.hpp b/src/examples/fake_imu/FakeImu.hpp index 489365557e..3fd614f4c4 100644 --- a/src/examples/fake_imu/FakeImu.hpp +++ b/src/examples/fake_imu/FakeImu.hpp @@ -69,7 +69,7 @@ public: bool init(); private: - static constexpr double IMU_RATE_HZ = 8000; + static constexpr int IMU_RATE_HZ = 8000; void Run() override; diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index fb7d9f0922..bbfdbba0e8 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -67,8 +67,8 @@ void FakeMagnetometer::Run() if (_vehicle_gps_position_sub.copy(&gps)) { if (gps.eph < 1000) { - const double lat = gps.lat / 1.e7; - const double lon = gps.lon / 1.e7; + const float lat = gps.lat / 1.e7f; + const float lon = gps.lon / 1.e7f; // magnetic field data returned by the geo library using the current GPS position const float mag_declination_gps = get_mag_declination_radians(lat, lon); diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index a903e24da2..2302f86a99 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -169,7 +169,7 @@ void Battery::sumDischarged(const hrt_abstime ×tamp, float current_a) // Ignore first update because we don't know dt. if (_last_timestamp != 0) { - const float dt = (timestamp - _last_timestamp) / 1e6; + const float dt = (timestamp - _last_timestamp) / 1e6f; // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); _discharged_mah += _discharged_mah_loop; diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b69..6f4f517610 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -124,7 +124,7 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); + msg_index = (int)ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { @@ -143,7 +143,7 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); + msg_index = (int)ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { @@ -295,7 +295,7 @@ void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { const float col_prev_d = _param_cp_dist.get(); - const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); + const int guidance_bins = (int)floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); const int sp_index_original = setpoint_index; float best_cost = 9999.f; int new_sp_index = setpoint_index; @@ -414,7 +414,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); - int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + int sp_index = (int)floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); diff --git a/src/lib/controllib/controllib_test/controllib_test_main.cpp b/src/lib/controllib/controllib_test/controllib_test_main.cpp index cbe816a76e..7dcd108076 100644 --- a/src/lib/controllib/controllib_test/controllib_test_main.cpp +++ b/src/lib/controllib/controllib_test/controllib_test_main.cpp @@ -447,7 +447,7 @@ int blockRandUniformTest() } ASSERT_CL(equal(mean, (blockRandUniform.getMin() + - blockRandUniform.getMax()) / 2, 1e-1)); + blockRandUniform.getMax()) / 2.f, 1e-1f)); printf("PASS\n"); return 0; } @@ -476,8 +476,8 @@ int blockRandGaussTest() float stdDev = sqrtf(sum / (n - 1)); (void)(stdDev); - ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1)); - ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1f)); + ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1f)); printf("PASS\n"); return 0; } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 80a9910d8b..3009a72347 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -379,9 +379,9 @@ __EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_ // otherwise use full rotation matrix for valid rotations if (rot < ROTATION_MAX) { const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; - x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); - y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); - z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); + x = (int16_t)math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); + y = (int16_t)math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); + z = (int16_t)math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); } } } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 497381e1e8..e7173038c0 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -56,7 +56,7 @@ static constexpr uint8_t clipping(const int16_t samples[], int16_t clip_limit, u unsigned clip_count = 0; for (int n = 0; n < len; n++) { - if (abs(samples[n]) >= clip_limit) { + if ((samples[n] <= clip_limit) || (samples[n] >= clip_limit)) { clip_count++; } } @@ -100,7 +100,7 @@ void PX4Accelerometer::set_scale(float scale) float rescale = _scale / scale; for (auto &s : _last_sample) { - s = roundf(s * rescale); + s = (int16_t)roundf(s * rescale); } _scale = scale; @@ -124,9 +124,9 @@ void PX4Accelerometer::update(const hrt_abstime ×tamp_sample, float x, floa report.x = x * _scale; report.y = y * _scale; report.z = z * _scale; - report.clip_counter[0] = (fabsf(x) >= _clip_limit); - report.clip_counter[1] = (fabsf(y) >= _clip_limit); - report.clip_counter[2] = (fabsf(z) >= _clip_limit); + report.clip_counter[0] = (fabsf(x) >= (float)_clip_limit); + report.clip_counter[1] = (fabsf(y) >= (float)_clip_limit); + report.clip_counter[2] = (fabsf(z) >= (float)_clip_limit); report.samples = 1; report.timestamp = hrt_absolute_time(); @@ -183,5 +183,5 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample) void PX4Accelerometer::UpdateClipLimit() { // 99.9% of potential max - _clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX); + _clip_limit = (int16_t)math::constrain((int16_t)(roundf((_range / _scale) * 0.999f)), (int16_t)0, (int16_t)INT16_MAX); } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 1e4885ccf7..3746f86c65 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -79,7 +79,7 @@ private: float _scale{1.f}; float _temperature{NAN}; - float _clip_limit{_range / _scale}; + int16_t _clip_limit{(int16_t)(_range / _scale)}; uint32_t _error_count{0}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index d816893e26..51b4013cf2 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -87,7 +87,7 @@ void PX4Gyroscope::set_scale(float scale) float rescale = _scale / scale; for (auto &s : _last_sample) { - s = roundf(s * rescale); + s = (int16_t)roundf(s * rescale); } _scale = scale; diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index 77d1991f53..3e020f6923 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -115,13 +115,13 @@ void __EXPORT float2SigExp(const float &num, float &sig, int &exp) return; } - exp = log10f(fabsf(num)); + exp = (int)log10f(fabsf(num)); if (exp > 0) { - exp = ceil(exp); + exp = (int)ceil(exp); } else { - exp = floor(exp); + exp = (int)floor(exp); } sig = num; diff --git a/src/lib/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp index 48b936b146..96e397c392 100644 --- a/src/lib/mathlib/math/test/test.hpp +++ b/src/lib/mathlib/math/test/test.hpp @@ -43,7 +43,7 @@ //#include //#include -bool equal(float a, float b, float eps = 1e-5); +bool equal(float a, float b, float eps = 1e-5f); bool greater_than(float a, float b); diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp index e202367e97..1d637ec87a 100644 --- a/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp +++ b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp @@ -201,7 +201,7 @@ HelicopterMixer::mix(float *outputs, unsigned space) /* Find index to use for curves */ float thrust_cmd = get_control(0, 3); - int idx = (thrust_cmd / 0.25f); + int idx = (int)roundf(thrust_cmd / 0.25f); /* Make sure idx is in range */ if (idx < 0) { diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index 3321b42253..e118b8106d 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -112,7 +112,7 @@ MixerGroup::get_trims(int16_t *values) // MultirotorMixer returns the number of motors so we // loop through index_mixer and set the same trim value for all motors while (index < index_mixer) { - values[index] = trim * 10000; + values[index] = (int16_t)roundf(trim * 10000.f); index++; } } diff --git a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py index 932a265fb4..1594c15d14 100755 --- a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py +++ b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py @@ -263,12 +263,12 @@ def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, for row in mix: if use_6dof: # 6dof mixer - buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + buf.write(u"\t{{ {:9f}f, {:9f}f, {:9f}f, {:9f}f, {:9f}f, {:9f}f }},\n".format( row[0], row[1], row[2], row[3], row[4], row[5])) else: # 4dof mixer - buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + buf.write(u"\t{{ {:9f}f, {:9f}f, {:9f}f, {:9f}f }},\n".format( row[0], row[1], row[2], -row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly diff --git a/src/lib/output_limit/CMakeLists.txt b/src/lib/output_limit/CMakeLists.txt index ac1152a9c3..dd83cf2a8e 100644 --- a/src/lib/output_limit/CMakeLists.txt +++ b/src/lib/output_limit/CMakeLists.txt @@ -32,5 +32,6 @@ ############################################################################ add_library(output_limit output_limit.cpp) +target_compile_options(output_limit PRIVATE -Wno-float-conversion) # TODO: fix and enable target_link_libraries(output_limit PRIVATE prebuild_targets) diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index ee4de14cec..71c13ccb83 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -1380,7 +1380,7 @@ param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node) goto out; } - f = node->d; + f = static_cast(node->d); v = &f; PX4_DEBUG("Imported %s with value %f", param_name(param), (double)f); diff --git a/src/lib/parameters/templates/px4_parameters.hpp.jinja b/src/lib/parameters/templates/px4_parameters.hpp.jinja index a16046007d..81619e4197 100644 --- a/src/lib/parameters/templates/px4_parameters.hpp.jinja +++ b/src/lib/parameters/templates/px4_parameters.hpp.jinja @@ -25,9 +25,9 @@ static constexpr param_info_s parameters[] = { { "{{ param.attrib["name"] }}", {%- if param.attrib["type"] == "FLOAT" %} - .val = {{ "{" }} .f = {{ param.attrib["default"] }} {{ "}" }}, + .val = {{ "{" }} .f = {{"(float)"}}{{ param.attrib["default"] }}{{ "}" }}, {%- elif param.attrib["type"] == "INT32" %} - .val = {{ "{" }} .i = {{ param.attrib["default"] }}{{ "}" }}, + .val = {{ "{" }} .i = {{"(int32_t)"}}{{ param.attrib["default"] }}{{ "}" }}, {%- endif %} }, {% endfor %} diff --git a/src/lib/rc/crsf.cpp b/src/lib/rc/crsf.cpp index 383a0fbe71..477afe8e70 100644 --- a/src/lib/rc/crsf.cpp +++ b/src/lib/rc/crsf.cpp @@ -44,6 +44,7 @@ #endif #include +#include #include #include #include @@ -222,7 +223,7 @@ static uint16_t convert_channel_value(unsigned chan_value) */ static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f); static constexpr float offset = 988.f - 172.f * scale; - return (scale * chan_value) + offset; + return (uint16_t)roundf((scale * static_cast(chan_value)) + offset); } static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels) diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index 6ed2ec413f..3b3501d937 100644 --- a/src/lib/rc/dsm.cpp +++ b/src/lib/rc/dsm.cpp @@ -132,7 +132,7 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u // PWM_OUT = (ServoPosition x 1.166μs) + Offset static constexpr uint16_t offset = 903; // microseconds - value = roundf(servo_position * 1.166f) + offset; + value = static_cast(roundf(servo_position * 1.166f)) + offset; // Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1) // ±100% travel is 1102µs to 1898 µs @@ -187,7 +187,7 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u // PWM_OUT = (ServoPosition x 0.583μs) + Offset static constexpr uint16_t offset = 903; // microseconds - value = roundf(servo_position * 0.583f) + offset; + value = (uint16_t)roundf(servo_position * 0.583f) + offset; // Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1) // ±100% travel is 1102µs to 1898 µs diff --git a/src/lib/rc/st24.cpp b/src/lib/rc/st24.cpp index 50a2f72769..21e50ae6b7 100644 --- a/src/lib/rc/st24.cpp +++ b/src/lib/rc/st24.cpp @@ -44,6 +44,8 @@ #include "st24.h" #include "common_rc.h" +#include + const char *decode_states[] = {"UNSYNCED", "GOT_STX1", "GOT_STX2", @@ -167,7 +169,7 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; // Scale from 0..255 to 100%. - *rssi = d->rssi * (100.0f / 255.0f); + *rssi = (uint8_t)math::constrain(static_cast(d->rssi) * (100.0f / 255.0f), 0.f, 255.f); *lost_count = d->lost_count; /* this can lead to rounding of the strides */ @@ -196,7 +198,7 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; // Scale from 0..255 to 100%. - *rssi = d->rssi * (100.0f / 255.0f); + *rssi = (uint8_t)math::constrain(static_cast(d->rssi) * (100.0f / 255.0f), 0.f, 255.f); *lost_count = d->lost_count; /* this can lead to rounding of the strides */ diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 0fc5e1ac9d..8230ebcf5b 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -107,8 +107,8 @@ public: // setters for failure detection tuning parameters void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; } void set_tas_innov_integ_threshold(float tas_innov_integ_threshold) { _tas_innov_integ_threshold = tas_innov_integ_threshold; } - void set_checks_fail_delay(float checks_fail_delay) { _checks_fail_delay = checks_fail_delay; } - void set_checks_clear_delay(float checks_clear_delay) { _checks_clear_delay = checks_clear_delay; } + void set_checks_fail_delay(float checks_fail_delay) { _checks_fail_delay = (int)roundf(checks_fail_delay); } + void set_checks_clear_delay(float checks_clear_delay) { _checks_clear_delay = (int)roundf(checks_clear_delay); } void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index dac73ae16d..8e14a02223 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -33,7 +33,7 @@ PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001); * @unit m/s * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4); +PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f); /** * Airspeed Selector: Wind estimator sideslip measurement noise @@ -45,7 +45,7 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4); * @unit rad * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3); +PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f); /** * Airspeed Selector: Gate size for true airspeed fusion diff --git a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp index 6541569be5..f6941a9405 100644 --- a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp +++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -76,7 +76,7 @@ void arm_auth_param_update() { float timeout = 0; param_get(param_find("COM_ARM_AUTH_TO"), &timeout); - _param_com_arm_auth_timout = timeout * 1_s; + _param_com_arm_auth_timout = static_cast(timeout * 1e6f); int32_t auth_method = ARM_AUTH_METHOD_ARM_REQ; param_get(param_find("COM_ARM_AUTH_MET"), &auth_method); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ec07706687..e5d2fa9f49 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -400,7 +400,7 @@ int Commander::custom_command(int argc, char *argv[]) double latitude = atof(argv[1]); double longitude = atof(argv[2]); - float altitude = atof(argv[3]); + float altitude = (float)atof(argv[3]); // Set the ekf NED origin global coordinates. bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN, @@ -708,7 +708,7 @@ Commander::Commander() : ModuleParams(nullptr), _failure_detector(this) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, static_cast(_param_com_disarm_preflight.get() * 1_s)); _land_detector.landed = true; @@ -1228,7 +1228,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - const int param1 = cmd.param1; + const int param1 = (int)roundf(cmd.param1); if (param1 == 0) { // 0: Do nothing for autopilot @@ -1850,7 +1850,7 @@ Commander::run() _flight_mode_slots[4] = _param_fltmode_5.get(); _flight_mode_slots[5] = _param_fltmode_6.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); + _auto_disarm_killed.set_hysteresis_time_from(false, static_cast(_param_com_kill_disarm.get() * 1_s)); /* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */ if (param_airmode != PARAM_INVALID && param_rc_map_arm_switch != PARAM_INVALID) { @@ -1869,7 +1869,7 @@ Commander::run() } } - _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1e6f); + _offboard_available.set_hysteresis_time_from(true, static_cast(_param_com_of_loss_t.get() * 1e6f)); param_init_forced = false; } @@ -2083,11 +2083,11 @@ Commander::run() if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, static_cast(_param_com_disarm_land.get() * 1_s)); _auto_disarm_landed.set_state_and_update(_land_detector.landed, hrt_absolute_time()); } else if (_param_com_disarm_preflight.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, static_cast(_param_com_disarm_preflight.get() * 1_s)); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } diff --git a/src/modules/commander/lm_fit.cpp b/src/modules/commander/lm_fit.cpp index 39eb571a79..3f6734220a 100644 --- a/src/modules/commander/lm_fit.cpp +++ b/src/modules/commander/lm_fit.cpp @@ -319,11 +319,11 @@ int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int s const int max_iterations = 100; const int min_iterations = 10; - const float cost_threshold = 0.01; - const float step_threshold = 0.001; + const float cost_threshold = 0.01f; + const float step_threshold = 0.001f; - const float min_radius = 0.2; - const float max_radius = 0.7; + const float min_radius = 0.2f; + const float max_radius = 0.7f; iteration_result iter; iter.cost = 1e30f; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 6900be8c04..d199e9191f 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -165,7 +165,7 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl static unsigned progress_percentage(mag_worker_data_t *worker_data) { - return 100 * ((float)worker_data->done_count) / worker_data->calibration_sides; + return 100 * (int)roundf((float)worker_data->done_count / (float)worker_data->calibration_sides); } // Returns calibrate_return_error if any parameter is not finite @@ -243,7 +243,7 @@ static float get_sphere_radius() const double lon = gps.lon / 1.e7; // magnetic field data returned by the geo library using the current GPS position - return get_mag_strength_gauss(lat, lon); + return get_mag_strength_gauss(static_cast(lat), static_cast(lon)); } } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5b099953d3..4922bd5e8f 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -287,7 +287,7 @@ struct parameters { // synthetic sideslip fusion float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) float beta_noise{0.3f}; ///< synthetic sideslip noise (rad) - const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) + const uint64_t beta_avg_ft_us{150000}; ///< The average time between synthetic sideslip measurements (uSec) // range finder fusion float range_noise{0.1f}; ///< observation noise for range finder measurements (m) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 305e5fc547..02d5920eb1 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -491,13 +491,13 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - _imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; + _imu_buffer_length = (uint8_t)ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; // set the observation buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter const float ekf_delay_ms = max_time_delay_ms * 1.5f; - _obs_buffer_length = ceilf(ekf_delay_ms / _params.sensor_interval_min_ms); + _obs_buffer_length = (uint8_t)ceilf(ekf_delay_ms / _params.sensor_interval_min_ms); // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) _obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length); diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 53aceafa26..caaf002fe1 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -86,9 +86,9 @@ bool Ekf::collect_gps(const gps_message &gps) const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps); // set the magnetic field data returned by the geo library using the current GPS position - _mag_declination_gps = get_mag_declination_radians(lat, lon); - _mag_inclination_gps = get_mag_inclination_radians(lat, lon); - _mag_strength_gps = get_mag_strength_gauss(lat, lon); + _mag_declination_gps = get_mag_declination_radians(static_cast(lat), static_cast(lon)); + _mag_inclination_gps = get_mag_inclination_radians(static_cast(lat), static_cast(lon)); + _mag_strength_gps = get_mag_strength_gauss(static_cast(lat), static_cast(lon)); // request a reset of the yaw using the new declination if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) @@ -119,9 +119,9 @@ bool Ekf::collect_gps(const gps_message &gps) const double lon = gps.lon * 1.0e-7; // set the magnetic field data returned by the geo library using the current GPS position - _mag_declination_gps = get_mag_declination_radians(lat, lon); - _mag_inclination_gps = get_mag_inclination_radians(lat, lon); - _mag_strength_gps = get_mag_strength_gauss(lat, lon); + _mag_declination_gps = get_mag_declination_radians(static_cast(lat), static_cast(lon)); + _mag_inclination_gps = get_mag_inclination_radians(static_cast(lat), static_cast(lon)); + _mag_strength_gps = get_mag_strength_gauss(static_cast(lat), static_cast(lon)); // request mag yaw reset if there's a mag declination for the first time if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 178f065488..402276748e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -262,7 +262,7 @@ void EKF2::Run() // update parameters from storage updateParams(); - _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); + _ekf.set_min_required_gps_health_time(static_cast(_param_ekf2_req_gps_h.get() * 1_s)); // The airspeed scale factor correcton is only available via parameter as used by the airspeed module param_t param_aspd_scale = param_find("ASPD_SCALE"); diff --git a/src/modules/ekf2/test/sensor_simulator/gps.cpp b/src/modules/ekf2/test/sensor_simulator/gps.cpp index 47bd63f71f..09e09166b6 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.cpp +++ b/src/modules/ekf2/test/sensor_simulator/gps.cpp @@ -87,7 +87,7 @@ void Gps::setPositionRateNED(const Vector3f &rate) void Gps::stepHeightByMeters(const float hgt_change) { - _gps_data.alt += hgt_change * 1e3f; + _gps_data.alt += (int32_t)(hgt_change * 1e3f); } void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change) diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp index 86d7379850..be8da4912b 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp @@ -254,16 +254,15 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample) _airspeed.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]); } else if (sample.sensor_type == sensor_info::RANGE) { - _rng.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]); + _rng.setData((float) sample.sensor_data[0], (int8_t) sample.sensor_data[1]); } else if (sample.sensor_type == sensor_info::FLOW) { flowSample flow_sample; - flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0], - sample.sensor_data[1]); - flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2], - sample.sensor_data[3], - sample.sensor_data[4]); - flow_sample.quality = sample.sensor_data[5]; + flow_sample.flow_xy_rad = Vector2f((float)sample.sensor_data[0], (float)sample.sensor_data[1]); + flow_sample.gyro_xyz = Vector3f((float)sample.sensor_data[2], + (float)sample.sensor_data[3], + (float)sample.sensor_data[4]); + flow_sample.quality = (int8_t)sample.sensor_data[5]; _flow.setData(flow_sample); } else if (sample.sensor_type == sensor_info::VISION) { diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 089d83154a..1d939a7f66 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -70,7 +70,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) // commanded heading behaviour if (PX4_ISFINITE(command.param3)) { - _yaw_behaviour = command.param3; + _yaw_behaviour = (int)roundf(command.param3); } // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 15921ef30d..d87cebebd2 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -362,9 +362,12 @@ bool MulticopterLandDetector::_is_close_to_ground() void MulticopterLandDetector::_set_hysteresis_factor(const int factor) { - _ground_contact_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); - _landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); - _maybe_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); + _ground_contact_hysteresis.set_hysteresis_time_from(false, + static_cast(_param_lndmc_trig_time.get() * 1_s / 3 * factor)); + _landed_hysteresis.set_hysteresis_time_from(false, + static_cast(_param_lndmc_trig_time.get() * 1_s / 3 * factor)); + _maybe_landed_hysteresis.set_hysteresis_time_from(false, + static_cast(_param_lndmc_trig_time.get() * 1_s / 3 * factor)); _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 7ee630e29c..3ac2ed541b 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -835,9 +835,9 @@ void BlockLocalPositionEstimator::initP() // use vxy thresh for vz init as well m_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get(); // initialize bias uncertainty to small values to keep them stable - m_P(X_bx, X_bx) = 1e-6; - m_P(X_by, X_by) = 1e-6; - m_P(X_bz, X_bz) = 1e-6; + m_P(X_bx, X_bx) = 1e-6f; + m_P(X_by, X_by) = 1e-6f; + m_P(X_bz, X_bz) = 1e-6f; m_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 9d1af11e96..baacd2701c 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -52,13 +52,13 @@ static const size_t N_DIST_SUBS = 4; // chi squared distribution, false alarm probability 0.0001 // see fault_table.py // note skip 0 index so we can use degree of freedom as index -static const float BETA_TABLE[7] = {0, - 8.82050518214, - 12.094592431, - 13.9876612368, - 16.0875642296, - 17.8797700658, - 19.6465647819, +static const float BETA_TABLE[7] = {0.f, + 8.82050518214f, + 12.094592431f, + 13.9876612368f, + 16.0875642296f, + 17.8797700658f, + 19.6465647819f, }; class BlockLocalPositionEstimator : public ModuleBase, public ModuleParams, diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 3ef5ffac2e..c8e9b1e194 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -40,7 +40,7 @@ void BlockLocalPositionEstimator::gpsInit() // get mean gps values double gpsLat = _gpsStats.getMean()(0); double gpsLon = _gpsStats.getMean()(1); - float gpsAlt = _gpsStats.getMean()(2); + float gpsAlt = (float)_gpsStats.getMean()(2); _sensorTimeout &= ~SENSOR_GPS; _sensorFault &= ~SENSOR_GPS; @@ -115,7 +115,7 @@ void BlockLocalPositionEstimator::gpsCorrect() // gps measurement in local frame double lat = y_global(Y_gps_x); double lon = y_global(Y_gps_y); - float alt = y_global(Y_gps_z); + float alt = (float)y_global(Y_gps_z); float px = 0; float py = 0; float pz = -(alt - _gpsAltOrigin); @@ -125,9 +125,9 @@ void BlockLocalPositionEstimator::gpsCorrect() y(Y_gps_x) = px; y(Y_gps_y) = py; y(Y_gps_z) = pz; - y(Y_gps_vx) = y_global(Y_gps_vx); - y(Y_gps_vy) = y_global(Y_gps_vy); - y(Y_gps_vz) = y_global(Y_gps_vz); + y(Y_gps_vx) = (float)y_global(Y_gps_vx); + y(Y_gps_vy) = (float)y_global(Y_gps_vy); + y(Y_gps_vz) = (float)y_global(Y_gps_vz); // gps measurement matrix, measures position and velocity Matrix C; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 047f5f56ea..4eecbc1401 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -259,7 +259,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) r = 1e6; } - log_interval = 1e6 / r; + log_interval = (int)roundf(1e6f / static_cast(r)); } break; diff --git a/src/modules/logger/util.cpp b/src/modules/logger/util.cpp index ec1e658d07..53fd8c628e 100644 --- a/src/modules/logger/util.cpp +++ b/src/modules/logger/util.cpp @@ -83,7 +83,7 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) vehicle_gps_position_s gps_pos; if (vehicle_gps_position_sub.copy(&gps_pos)) { - utc_time_sec = gps_pos.time_utc_usec / 1e6; + utc_time_sec = gps_pos.time_utc_usec / 1000000; if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) { use_clock_time = false; @@ -94,7 +94,7 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) /* take clock time if there's no fix (yet) */ struct timespec ts = {}; px4_clock_gettime(CLOCK_REALTIME, &ts); - utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + utc_time_sec = ts.tv_sec + (ts.tv_nsec / (1'000'000'000'000)); if (utc_time_sec < GPS_EPOCH_SECS) { return false; @@ -103,7 +103,7 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) /* strip the time elapsed since boot */ if (boot_time) { - utc_time_sec -= hrt_absolute_time() / 1e6; + utc_time_sec -= hrt_absolute_time() / 1'000'000; } /* apply utc offset */ diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index cd7135338b..7b2f4f4b39 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable -Wno-address-of-packed-member # TODO: fix in c_library_v2 -Wno-enum-compare # ROTATION <-> MAV_SENSOR_ROTATION + -DMAVLINK_NO_CONVERSION_HELPERS #-DDEBUG_BUILD INCLUDES ${PX4_SOURCE_DIR}/mavlink/include/mavlink diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 6b00fd4e49..026ede78c7 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -85,7 +85,7 @@ MavlinkStream::update(const hrt_abstime &t) int interval = _interval; if (!const_rate()) { - interval /= _mavlink->get_rate_mult(); + interval = static_cast(static_cast(interval) / _mavlink->get_rate_mult()); } // We don't need to send anything if the inverval is 0. send() will be called manually. diff --git a/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/modules/mavlink/mavlink_tests/CMakeLists.txt index da27da0f20..5a776144c7 100644 --- a/src/modules/mavlink/mavlink_tests/CMakeLists.txt +++ b/src/modules/mavlink/mavlink_tests/CMakeLists.txt @@ -45,6 +45,7 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable -Wno-address-of-packed-member # TODO: fix in c_library_v2 -Wno-double-promotion # The fix has been proposed as PR upstream (2020-03-08) + -DMAVLINK_NO_CONVERSION_HELPERS SRCS mavlink_tests.cpp mavlink_ftp_test.cpp diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index ab2a39ffc9..c19cd3fb3e 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -60,7 +60,7 @@ public: ~Takeoff() = default; // initialize parameters - void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, static_cast(seconds * 1e6f)); } void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } /** diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index aa14669c05..4b972aea80 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -198,8 +198,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const veh return checkAll(global_position); } else { - return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3f); } } else { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 389aa795a2..5d856f43dd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1892,8 +1892,8 @@ void Mission::publish_navigator_mission_item() navigator_mission_item.instance_count = _navigator->mission_instance_count(); navigator_mission_item.sequence_current = _current_mission_index; navigator_mission_item.nav_cmd = _mission_item.nav_cmd; - navigator_mission_item.latitude = _mission_item.lat; - navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.latitude = (float)_mission_item.lat; + navigator_mission_item.longitude = (float)_mission_item.lon; navigator_mission_item.altitude = _mission_item.altitude; navigator_mission_item.time_inside = get_time_inside(_mission_item); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ae922ed802..5782c7b0a6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -432,7 +432,7 @@ Navigator::run() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { - if (!_mission.set_current_mission_index(cmd.param1)) { + if (!_mission.set_current_mission_index((int)roundf(cmd.param1))) { PX4_WARN("CMD_MISSION_START failed"); } } @@ -469,7 +469,7 @@ Navigator::run() switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: case vehicle_command_s::VEHICLE_CMD_NAV_ROI: - _vroi.mode = cmd.param1; + _vroi.mode = (int)roundf(cmd.param1); break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: @@ -1217,7 +1217,7 @@ void Navigator::check_traffic() if (action_needed) { // direction of traffic in human-readable 0..360 degree in earth frame - int traffic_direction = math::degrees(tr.heading) + 180; + int traffic_direction = (int)math::degrees(tr.heading) + 180; int traffic_seperation = (int)fabsf(cr.distance); switch (_param_nav_traff_avoid.get()) { diff --git a/src/modules/rc_update/CMakeLists.txt b/src/modules/rc_update/CMakeLists.txt index c6ba78dcce..669f2efa15 100644 --- a/src/modules/rc_update/CMakeLists.txt +++ b/src/modules/rc_update/CMakeLists.txt @@ -34,6 +34,8 @@ px4_add_module( MODULE modules__rc_update MAIN rc_update + COMPILE_FLAGS + -Wno-float-conversion # TODO: fix and enable SRCS rc_update.cpp rc_update.h diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 0d2e302366..04e20e551b 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -127,15 +127,15 @@ void RCUpdate::parameters_updated() float min = 0.f; param_get(_parameter_handles.min[i], &min); - _parameters.min[i] = min; + _parameters.min[i] = math::constrain((uint16_t)roundf(min), (uint16_t)0, (uint16_t)UINT16_MAX); float trim = 0.f; param_get(_parameter_handles.trim[i], &trim); - _parameters.trim[i] = trim; + _parameters.trim[i] = math::constrain((uint16_t)roundf(trim), (uint16_t)0, (uint16_t)UINT16_MAX); float max = 0.f; param_get(_parameter_handles.max[i], &max); - _parameters.max[i] = max; + _parameters.max[i] = math::constrain((uint16_t)roundf(max), (uint16_t)0, (uint16_t)UINT16_MAX); float rev = 0.f; param_get(_parameter_handles.rev[i], &rev); @@ -143,7 +143,7 @@ void RCUpdate::parameters_updated() float dz = 0.f; param_get(_parameter_handles.dz[i], &dz); - _parameters.dz[i] = dz; + _parameters.dz[i] = math::constrain((uint16_t)roundf(dz), (uint16_t)0, (uint16_t)UINT16_MAX); } for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 76d17430cf..bccaf17cb3 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -810,7 +810,7 @@ Replay::run() const char *speedup = getenv("PX4_SIM_SPEED_FACTOR"); if (speedup) { - _speed_factor = atof(speedup); + _speed_factor = (float)atof(speedup); } onEnterMainLoop(); @@ -960,7 +960,7 @@ Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset) _accumulated_delay += (publish_timestamp - cur_time) / _speed_factor; if (_accumulated_delay > 3000) { - system_usleep(_accumulated_delay); + system_usleep((int)_accumulated_delay); _accumulated_delay = 0.f; } } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index f45ac9e189..b047e6866f 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -199,7 +199,7 @@ bool RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) { - float dt = 0.01; // Using non zero value to a avoid division by zero + float dt = 0.01f; // Using non zero value to a avoid division by zero if (_control_position_last_called > 0) { dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f; @@ -330,7 +330,7 @@ void RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) { const Vector3f desired_velocity{_trajectory_setpoint.vx, _trajectory_setpoint.vy, _trajectory_setpoint.vz}; - float dt = 0.01; // Using non zero value to a avoid division by zero + float dt = 0.01f; // Using non zero value to a avoid division by zero const float mission_throttle = _param_throttle_cruise.get(); const float desired_speed = desired_velocity.norm(); diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 02c6c0db79..9be2e6f835 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -103,8 +103,8 @@ void VehicleAcceleration::CheckAndUpdateFilters() if (_param_imu_integ_rate.get() > 0) { const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); const float sample_interval_avg = 1e6f / sample_rate_hz; - const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, - (float)sensor_accel_s::ORB_QUEUE_LENGTH); + const uint8_t samples = (uint8_t)math::constrain(roundf(configured_interval_us / sample_interval_avg), + 1.f, (float)sensor_accel_s::ORB_QUEUE_LENGTH); _sensor_sub.set_required_updates(samples); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index b861e93c2e..20ce5accdc 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -120,7 +120,7 @@ bool VehicleAngularVelocity::UpdateSampleRate() const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get(); const float publish_interval_us = 1e6f / publish_rate_hz; - const uint8_t samples = roundf(configured_interval_us / publish_interval_us); + const uint8_t samples = (uint8_t)roundf(configured_interval_us / publish_interval_us); if (_fifo_available) { _sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH)); @@ -435,7 +435,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) if (_esc_status_sub.copy(&esc_status) && (hrt_elapsed_time(&esc_status.timestamp) < DYNAMIC_NOTCH_FITLER_TIMEOUT)) { static constexpr int32_t ESC_RPM_MIN = 20 * 60; // TODO: configurable - const int32_t ESC_RPM_MAX = roundf(_filter_sample_rate_hz / 3.f * 60.f); // upper bound safety (well below Nyquist) + const int32_t ESC_RPM_MAX = (int32_t)roundf(_filter_sample_rate_hz / 3.f * + 60.f); // upper bound safety (well below Nyquist) for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESC_RPM); esc++) { @@ -712,7 +713,7 @@ void VehicleAngularVelocity::Run() if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) { if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { - _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; + _timestamp_sample_last = sensor_data.timestamp_sample - (uint64_t)roundf(1e6f / _filter_sample_rate_hz); } const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f); diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/vehicle_imu/Integrator.hpp index 646e305f41..8d9e661d30 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -110,7 +110,7 @@ public: { if (integral_ready()) { integral = _alpha; - integral_dt = roundf(_integral_dt * 1e6f); // seconds to microseconds + integral_dt = (uint16_t)roundf(_integral_dt * 1e6f); // seconds to microseconds reset(); diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index bf2862c267..023e4f5b12 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -53,7 +53,7 @@ VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, co _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), _instance(instance) { - _imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); + _imu_integration_interval_us = (uint32_t)roundf(1e6f / static_cast(_param_imu_integ_rate.get())); _accel_integrator.set_reset_interval(_imu_integration_interval_us); _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); @@ -128,7 +128,7 @@ void VehicleIMU::ParametersUpdate(bool force) _param_imu_integ_rate.commit_no_notification(); } - _imu_integration_interval_us = 1e6f / imu_integration_rate_hz; + _imu_integration_interval_us = (uint32_t)roundf(1e6f / static_cast(imu_integration_rate_hz)); if (_param_imu_integ_rate.get() != imu_integ_rate_prev) { // force update @@ -323,9 +323,9 @@ bool VehicleIMU::UpdateAccel() Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; // round to get reasonble clip counts per axis (after board rotation) - const uint8_t clip_x = roundf(fabsf(clipping(0))); - const uint8_t clip_y = roundf(fabsf(clipping(1))); - const uint8_t clip_z = roundf(fabsf(clipping(2))); + const uint8_t clip_x = (uint8_t)roundf(fabsf(clipping(0))); + const uint8_t clip_y = (uint8_t)roundf(fabsf(clipping(1))); + const uint8_t clip_z = (uint8_t)roundf(fabsf(clipping(2))); _status.accel_clipping[0] += clip_x; _status.accel_clipping[1] += clip_y; @@ -558,21 +558,21 @@ void VehicleIMU::UpdateIntegratorConfiguration() gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us / 2) * 2); } - uint32_t integration_interval_us = roundf(gyro_integral_samples * _gyro_interval_us); + uint32_t integration_interval_us = (uint32_t)roundf(gyro_integral_samples * _gyro_interval_us); // accel follows gyro as closely as possible uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / _accel_interval_us)); // let the gyro set the configuration and scheduling // relaxed minimum integration time required - _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us)); + _accel_integrator.set_reset_interval((uint32_t)roundf((accel_integral_samples - 0.5f) * _accel_interval_us)); _accel_integrator.set_reset_samples(accel_integral_samples); - _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us)); + _gyro_integrator.set_reset_interval((uint32_t)roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us)); _gyro_integrator.set_reset_samples(gyro_integral_samples); - _backup_schedule_timeout_us = math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, - sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us); + _backup_schedule_timeout_us = (uint32_t)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, + sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us); // gyro: find largest integer multiple of gyro_integral_samples for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index d19927f47f..b6b0003607 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -54,6 +54,7 @@ px4_add_module( -Wno-double-promotion -Wno-cast-align -Wno-address-of-packed-member # TODO: fix in c_library_v2 + -DMAVLINK_NO_CONVERSION_HELPERS INCLUDES ${PX4_SOURCE_DIR}/mavlink/include/mavlink SRCS diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index dad520015e..2f4f05c3ea 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -227,9 +227,9 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor _last_accel_fifo.samples = 1; _last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample; _last_accel_fifo.timestamp_sample = time; - _last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE; - _last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE; - _last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE; + _last_accel_fifo.x[0] = (int16_t)roundf(sensors.xacc / ACCEL_FIFO_SCALE); + _last_accel_fifo.y[0] = (int16_t)roundf(sensors.yacc / ACCEL_FIFO_SCALE); + _last_accel_fifo.z[0] = (int16_t)roundf(sensors.zacc / ACCEL_FIFO_SCALE); _px4_accel[i].updateFIFO(_last_accel_fifo); } @@ -267,9 +267,9 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor _last_gyro_fifo.samples = 1; _last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample; _last_gyro_fifo.timestamp_sample = time; - _last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE; - _last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE; - _last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE; + _last_gyro_fifo.x[0] = (int16_t)roundf(sensors.xgyro / GYRO_FIFO_SCALE); + _last_gyro_fifo.y[0] = (int16_t)roundf(sensors.ygyro / GYRO_FIFO_SCALE); + _last_gyro_fifo.z[0] = (int16_t)roundf(sensors.zgyro / GYRO_FIFO_SCALE); _px4_gyro[i].updateFIFO(_last_gyro_fifo); } @@ -526,9 +526,9 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg { hil_gpos.timestamp = timestamp; - hil_gpos.lat = hil_state.lat / 1E7;//1E7 - hil_gpos.lon = hil_state.lon / 1E7;//1E7 - hil_gpos.alt = hil_state.alt / 1E3;//1E3 + hil_gpos.lat = hil_state.lat / 1e7;//1E7 + hil_gpos.lon = hil_state.lon / 1e7;//1E7 + hil_gpos.alt = hil_state.alt / 1e3f;//1E3 // always publish ground truth attitude message _gpos_ground_truth_pub.publish(hil_gpos); diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 1c0fb12a61..f4a1942ee2 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -716,8 +716,8 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { - const int param_sys_id = roundf(vehicle_command.param1); - const int param_comp_id = roundf(vehicle_command.param2); + const int param_sys_id = (int)roundf(vehicle_command.param1); + const int param_comp_id = (int)roundf(vehicle_command.param2); uint8_t new_sys_id_primary_control = [&]() { if (param_sys_id >= 0 && param_sys_id < 256) { @@ -798,7 +798,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); const matrix::Quatf q(euler); const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4); - const uint32_t flags = vehicle_command.param5; + const uint32_t flags = (uint32_t)roundf(vehicle_command.param5); // TODO: support gimbal device id for multiple gimbals