Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: simulated SF45b fixes and enhancements 

resolve crash if time_delta_ms is too long
correct sample_count calculation
sends one reading per 3deg (closer to real device)
distance max is 53m
returns -1m on failure
sweeps back and forth -190~190 deg
This commit is contained in:
Randy Mackay 2023-06-01 12:47:43 +09:00 committed by Peter Barker
parent bd6198ae1a
commit 29292c2aaa
2 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/SITL/SIM_PS_LightWare_SF45B.cpp
View File

@ -183,17 +183,17 @@ void PS_LightWare_SF45B::update_output_responses()
void PS_LightWare_SF45B::update_output_scan(const Location &location) void PS_LightWare_SF45B::update_output_scan(const Location &location)
{ {
const uint32_t now = AP_HAL::millis(); const uint32_t now_ms = AP_HAL::millis();
if (last_scan_output_time_ms == 0) { const uint32_t time_delta_ms = (now_ms - last_scan_output_time_ms);
last_scan_output_time_ms = now; if (time_delta_ms > 1000) {
last_scan_output_time_ms = now_ms;
return; return;
} }
const uint32_t time_delta = (now - last_scan_output_time_ms);
const uint32_t samples_per_second = 1000; const uint32_t samples_per_second = 133;
const float samples_per_ms = samples_per_second / 1000.0f; const float samples_per_ms = samples_per_second / 1000.0f;
const uint32_t sample_count = time_delta / samples_per_ms; const uint32_t sample_count = samples_per_ms * time_delta_ms;
const float degrees_per_ms = 1000 / 1000.0f; // Randy reports 1 degree increments const float degrees_per_ms = 390 / 1000.0f;
const float degrees_per_sample = degrees_per_ms / samples_per_ms; const float degrees_per_sample = degrees_per_ms / samples_per_ms;
// ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta, degrees_per_sample); // ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta, degrees_per_sample);
@ -201,16 +201,27 @@ void PS_LightWare_SF45B::update_output_scan(const Location &location)
last_scan_output_time_ms += sample_count/samples_per_ms; last_scan_output_time_ms += sample_count/samples_per_ms;
for (uint32_t i=0; i<sample_count; i++) { for (uint32_t i=0; i<sample_count; i++) {
const float current_degrees_bf = fmod((last_degrees_bf + degrees_per_sample), 360.0f);
const float ANGLE_MIN_DEG = -170;
const float ANGLE_MAX_DEG = +170;
float current_degrees_bf = last_degrees_bf + (last_dir * degrees_per_sample);
if (current_degrees_bf < ANGLE_MIN_DEG) {
current_degrees_bf += (ANGLE_MIN_DEG - current_degrees_bf);
last_dir = -last_dir;
}
if (current_degrees_bf > ANGLE_MAX_DEG) {
current_degrees_bf += (ANGLE_MAX_DEG - current_degrees_bf);
last_dir = -last_dir;
}
last_degrees_bf = current_degrees_bf; last_degrees_bf = current_degrees_bf;
const float MAX_RANGE = 16.0f; const float MAX_RANGE = 53.0f;
float distance = measure_distance_at_angle_bf(location, current_degrees_bf); float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
// ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance); // ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
if (distance > MAX_RANGE) { if (distance > MAX_RANGE) {
// sensor returns zero for out-of-range // sensor returns -1 for out-of-range
distance = 0.0f; distance = -1.0f;
} }
PackedMessage<DistanceDataCM> packed_distance_data { PackedMessage<DistanceDataCM> packed_distance_data {

3
libraries/SITL/SIM_PS_LightWare_SF45B.h
View File

@ -259,7 +259,8 @@ private:
uint32_t last_scan_output_time_ms; uint32_t last_scan_output_time_ms;
float last_degrees_bf; float last_degrees_bf; // previous iteration's lidar angle
float last_dir = 1; // previous iterations movement direction. +1 CW, -1 for CCW
}; };