aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: use NaN for invalid rangefinder data

Browse Source 
needed to cope properly with terrain errors leading to negative
rangefinder data
This commit is contained in:
Andrew Tridgell 2023-08-21 10:51:13 +10:00 committed by Peter Barker
parent f076f7c4f1
commit 1b6c329de5
5 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/SIM_Aircraft.cpp
View File

@ -71,9 +71,9 @@ Aircraft::Aircraft(const char *frame_str) :
sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed(); sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
} }
// init rangefinder array to -1 to signify no data // init rangefinder array to NaN to signify no data
for (uint8_t i = 0; i < ARRAY_SIZE(rangefinder_m); i++){ for (uint8_t i = 0; i < ARRAY_SIZE(rangefinder_m); i++){
rangefinder_m[i] = -1.0f; rangefinder_m[i] = nanf("");
} }
} }

4
libraries/SITL/SIM_DroneCANDevice.cpp
View File

@ -217,9 +217,9 @@ void DroneCANDevice::update_rangefinder() {
msg.sensor_id = 0; msg.sensor_id = 0;
msg.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; msg.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
const float dist = AP::sitl()->get_rangefinder(0); const float dist = AP::sitl()->get_rangefinder(0);
if (is_positive(dist)) { if (!isnan(dist)) {
msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
msg.range = dist; msg.range = MAX(0, dist);
} else { } else {
msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
msg.range = 0; msg.range = 0;

2
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -556,7 +556,7 @@ void FlightAxis::update(const struct sitl_input &input)
if (is_positive(dcm.c.z)) { if (is_positive(dcm.c.z)) {
rangefinder_m[0] = state.m_altitudeAGL_MTR / dcm.c.z; rangefinder_m[0] = state.m_altitudeAGL_MTR / dcm.c.z;
} else { } else {
rangefinder_m[0] = -1; rangefinder_m[0] = nanf("");
} }
report_FPS(); report_FPS();

3
libraries/SITL/SIM_Frame.cpp
View File

@ -553,13 +553,14 @@ void Frame::calculate_forces(const Aircraft &aircraft,
Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef(); Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
const auto *_sitl = AP::sitl();
for (uint8_t i=0; i<num_motors; i++) { for (uint8_t i=0; i<num_motors; i++) {
Vector3f mtorque, mthrust; Vector3f mtorque, mthrust;
motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage(), use_drag); motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage(), use_drag);
torque += mtorque; torque += mtorque;
thrust += mthrust; thrust += mthrust;
// simulate motor rpm // simulate motor rpm
if (!is_zero(AP::sitl()->vibe_motor)) { if (!is_zero(_sitl->vibe_motor)) {
rpm[motor_offset+i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f; rpm[motor_offset+i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f;
} }
} }

2
libraries/SITL/SITL.cpp
View File

@ -1021,7 +1021,7 @@ float SIM::get_rangefinder(uint8_t instance) {
if (instance < ARRAY_SIZE(state.rangefinder_m)) { if (instance < ARRAY_SIZE(state.rangefinder_m)) {
return state.rangefinder_m[instance]; return state.rangefinder_m[instance];
} }
return -1; return nanf("");
}; };
float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const