forked from Archive/PX4-Autopilot
Fix implicit float to double conversions
The both results of ?: should be of same type, and some compilers give error on this: " implicit conversion from 'float' to 'double' to match other result of conditional" Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
parent
62dc926891
commit
532f370e7d
|
@ -271,7 +271,7 @@ bool FlightTaskAuto::_evaluateTriplets()
|
||||||
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
|
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
|
||||||
_triplet_next_wp,
|
_triplet_next_wp,
|
||||||
_sub_triplet_setpoint.get().next.yaw,
|
_sub_triplet_setpoint.get().next.yaw,
|
||||||
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
|
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
|
||||||
_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
|
_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
|
||||||
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
|
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
|
||||||
}
|
}
|
||||||
|
|
|
@ -122,7 +122,7 @@ void LandDetector::Run()
|
||||||
const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
|
const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
|
||||||
const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state();
|
const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state();
|
||||||
const bool landDetected = _landed_hysteresis.get_state();
|
const bool landDetected = _landed_hysteresis.get_state();
|
||||||
const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY;
|
const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : (float)INFINITY;
|
||||||
const bool in_ground_effect = _ground_effect_hysteresis.get_state();
|
const bool in_ground_effect = _ground_effect_hysteresis.get_state();
|
||||||
|
|
||||||
// publish at 1 Hz, very first time, or when the result has changed
|
// publish at 1 Hz, very first time, or when the result has changed
|
||||||
|
|
|
@ -855,17 +855,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||||
const uint16_t type_mask = target_local_ned.type_mask;
|
const uint16_t type_mask = target_local_ned.type_mask;
|
||||||
|
|
||||||
if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) {
|
if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) {
|
||||||
setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? NAN : target_local_ned.x;
|
setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
|
||||||
setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? NAN : target_local_ned.y;
|
setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
|
||||||
setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? NAN : target_local_ned.z;
|
setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;
|
||||||
|
|
||||||
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? NAN : target_local_ned.vx;
|
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
|
||||||
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? NAN : target_local_ned.vy;
|
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
|
||||||
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? NAN : target_local_ned.vz;
|
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;
|
||||||
|
|
||||||
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? NAN : target_local_ned.afx;
|
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_local_ned.afx;
|
||||||
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? NAN : target_local_ned.afy;
|
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_local_ned.afy;
|
||||||
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? NAN : target_local_ned.afz;
|
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)NAN : target_local_ned.afz;
|
||||||
|
|
||||||
} else if (target_local_ned.coordinate_frame == MAV_FRAME_BODY_NED) {
|
} else if (target_local_ned.coordinate_frame == MAV_FRAME_BODY_NED) {
|
||||||
|
|
||||||
|
@ -927,8 +927,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||||
setpoint.thrust[1] = NAN;
|
setpoint.thrust[1] = NAN;
|
||||||
setpoint.thrust[2] = NAN;
|
setpoint.thrust[2] = NAN;
|
||||||
|
|
||||||
setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? NAN : target_local_ned.yaw;
|
setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? (float)NAN : target_local_ned.yaw;
|
||||||
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? NAN : target_local_ned.yaw_rate;
|
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_local_ned.yaw_rate;
|
||||||
|
|
||||||
|
|
||||||
offboard_control_mode_s ocm{};
|
offboard_control_mode_s ocm{};
|
||||||
|
@ -1038,21 +1038,21 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
|
||||||
}
|
}
|
||||||
|
|
||||||
// velocity
|
// velocity
|
||||||
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? NAN : target_global_int.vx;
|
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
|
||||||
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? NAN : target_global_int.vy;
|
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
|
||||||
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? NAN : target_global_int.vz;
|
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;
|
||||||
|
|
||||||
// acceleration
|
// acceleration
|
||||||
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? NAN : target_global_int.afx;
|
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_global_int.afx;
|
||||||
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? NAN : target_global_int.afy;
|
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_global_int.afy;
|
||||||
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? NAN : target_global_int.afz;
|
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)NAN : target_global_int.afz;
|
||||||
|
|
||||||
setpoint.thrust[0] = NAN;
|
setpoint.thrust[0] = NAN;
|
||||||
setpoint.thrust[1] = NAN;
|
setpoint.thrust[1] = NAN;
|
||||||
setpoint.thrust[2] = NAN;
|
setpoint.thrust[2] = NAN;
|
||||||
|
|
||||||
setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? NAN : target_global_int.yaw;
|
setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? (float)NAN : target_global_int.yaw;
|
||||||
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? NAN : target_global_int.yaw_rate;
|
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_global_int.yaw_rate;
|
||||||
|
|
||||||
|
|
||||||
offboard_control_mode_s ocm{};
|
offboard_control_mode_s ocm{};
|
||||||
|
@ -1397,7 +1397,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||||
|
|
||||||
// TODO: review use case
|
// TODO: review use case
|
||||||
attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ?
|
attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ?
|
||||||
NAN : attitude_target.body_yaw_rate;
|
(float)NAN : attitude_target.body_yaw_rate;
|
||||||
|
|
||||||
if (!thrust_body && !(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
|
if (!thrust_body && !(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
|
||||||
fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust);
|
fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust);
|
||||||
|
@ -1431,9 +1431,12 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||||
|
|
||||||
} else if (body_rates) {
|
} else if (body_rates) {
|
||||||
vehicle_rates_setpoint_s setpoint{};
|
vehicle_rates_setpoint_s setpoint{};
|
||||||
setpoint.roll = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) ? NAN : attitude_target.body_roll_rate;
|
setpoint.roll = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) ? (float)NAN :
|
||||||
setpoint.pitch = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) ? NAN : attitude_target.body_pitch_rate;
|
attitude_target.body_roll_rate;
|
||||||
setpoint.yaw = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? NAN : attitude_target.body_yaw_rate;
|
setpoint.pitch = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) ? (float)NAN :
|
||||||
|
attitude_target.body_pitch_rate;
|
||||||
|
setpoint.yaw = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN :
|
||||||
|
attitude_target.body_yaw_rate;
|
||||||
|
|
||||||
if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
|
if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
|
||||||
fill_thrust(setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust);
|
fill_thrust(setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust);
|
||||||
|
@ -2227,7 +2230,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
||||||
gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
|
gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
|
||||||
gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
|
gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
|
||||||
gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
|
gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
|
||||||
gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
|
gps.cog_rad = ((hil_gps.cog == 65535) ? (float)NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
|
||||||
gps.vel_ned_valid = true;
|
gps.vel_ned_valid = true;
|
||||||
|
|
||||||
gps.timestamp_time_relative = 0;
|
gps.timestamp_time_relative = 0;
|
||||||
|
|
|
@ -77,7 +77,7 @@ private:
|
||||||
|
|
||||||
vehicle_local_position_s lpos{};
|
vehicle_local_position_s lpos{};
|
||||||
_local_pos_sub.copy(&lpos);
|
_local_pos_sub.copy(&lpos);
|
||||||
msg.wind_alt = (lpos.z_valid && lpos.z_global) ? (-lpos.z + lpos.ref_alt) : NAN;
|
msg.wind_alt = (lpos.z_valid && lpos.z_global) ? (-lpos.z + lpos.ref_alt) : (float)NAN;
|
||||||
|
|
||||||
msg.horiz_accuracy = 0.0f;
|
msg.horiz_accuracy = 0.0f;
|
||||||
msg.vert_accuracy = 0.0f;
|
msg.vert_accuracy = 0.0f;
|
||||||
|
|
Loading…
Reference in New Issue