forked from Archive/PX4-Autopilot
WIP: enable -Wfloat-conversion everywhere
This commit is contained in:
Daniel Agar
parent
3de5623556
commit
f9973e0464
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<float>(cmd.param5);
|
||||
_CAMPOS_pitch_angle = static_cast<float>(cmd.param6);
|
||||
_CAMPOS_angle_interval = _CAMPOS_roll_angle * 2 / (_CAMPOS_num_poses - 1);
|
||||
_CAMPOS_pose_counter = 0;
|
||||
_CAMPOS_updated_roll_angle = false;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ int rpm_simulator_main(int argc, char *argv[])
|
|||
|
||||
uORB::Publication<rpm_s> 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;
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
{
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<double>(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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@
|
|||
//#include <time.h>
|
||||
//#include <stdlib.h>
|
||||
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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++;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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<float>(node->d);
|
||||
v = &f;
|
||||
|
||||
PX4_DEBUG("Imported %s with value %f", param_name(param), (double)f);
|
||||
|
|
|
|||
|
|
@ -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 %}
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@
|
|||
#endif
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
#include <termios.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
|
@ -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<float>(chan_value)) + offset);
|
||||
}
|
||||
|
||||
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
|
||||
|
|
|
|||
|
|
@ -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<uint16_t>(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
|
||||
|
|
|
|||
|
|
@ -44,6 +44,8 @@
|
|||
#include "st24.h"
|
||||
#include "common_rc.h"
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
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<float>(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<float>(d->rssi) * (100.0f / 255.0f), 0.f, 255.f);
|
||||
*lost_count = d->lost_count;
|
||||
|
||||
/* this can lead to rounding of the strides */
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<uint64_t>(timeout * 1e6f);
|
||||
|
||||
int32_t auth_method = ARM_AUTH_METHOD_ARM_REQ;
|
||||
param_get(param_find("COM_ARM_AUTH_MET"), &auth_method);
|
||||
|
|
|
|||
|
|
@ -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<uint64_t>(_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<uint64_t>(_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<uint64_t>(_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<uint64_t>(_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<uint64_t>(_param_com_disarm_preflight.get() * 1_s));
|
||||
_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<float>(lat), static_cast<float>(lon));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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<float>(lat), static_cast<float>(lon));
|
||||
_mag_inclination_gps = get_mag_inclination_radians(static_cast<float>(lat), static_cast<float>(lon));
|
||||
_mag_strength_gps = get_mag_strength_gauss(static_cast<float>(lat), static_cast<float>(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<float>(lat), static_cast<float>(lon));
|
||||
_mag_inclination_gps = get_mag_inclination_radians(static_cast<float>(lat), static_cast<float>(lon));
|
||||
_mag_strength_gps = get_mag_strength_gauss(static_cast<float>(lat), static_cast<float>(lon));
|
||||
|
||||
// request mag yaw reset if there's a mag declination for the first time
|
||||
if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
|
||||
|
|
|
|||
|
|
@ -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<uint32_t>(_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");
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<uint64_t>(_param_lndmc_trig_time.get() * 1_s / 3 * factor));
|
||||
_landed_hysteresis.set_hysteresis_time_from(false,
|
||||
static_cast<uint64_t>(_param_lndmc_trig_time.get() * 1_s / 3 * factor));
|
||||
_maybe_landed_hysteresis.set_hysteresis_time_from(false,
|
||||
static_cast<uint64_t>(_param_lndmc_trig_time.get() * 1_s / 3 * factor));
|
||||
_freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<BlockLocalPositionEstimator>, public ModuleParams,
|
||||
|
|
|
|||
|
|
@ -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<float, n_y_gps, n_x> C;
|
||||
|
|
|
|||
|
|
@ -259,7 +259,7 @@ Logger *Logger::instantiate(int argc, char *argv[])
|
|||
r = 1e6;
|
||||
}
|
||||
|
||||
log_interval = 1e6 / r;
|
||||
log_interval = (int)roundf(1e6f / static_cast<float>(r));
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ MavlinkStream::update(const hrt_abstime &t)
|
|||
int interval = _interval;
|
||||
|
||||
if (!const_rate()) {
|
||||
interval /= _mavlink->get_rate_mult();
|
||||
interval = static_cast<int>(static_cast<float>(interval) / _mavlink->get_rate_mult());
|
||||
}
|
||||
|
||||
// We don't need to send anything if the inverval is 0. send() will be called manually.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<uint64_t>(seconds * 1e6f)); }
|
||||
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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++) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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<float>(_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<float>(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--) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue