Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

updating A121 rangefinder driver #3

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
+234 −3
Draft

updating A121 rangefinder driver #3

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
189b2c9
updating
Sep 27, 2024
f93139b
cpp structure?
Sep 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion range_sensor/Core/Inc/range_sensor.hpp
View file
Original file line number Diff line number Diff line change
@@ -22,13 +22,24 @@ typedef struct
acc_detector_cal_result_dynamic_t detector_cal_result_dynamic;
} distance_detector_resources_t;

void read_radar (void);

class Rangefinder {
public:

void set_config(acc_detector_distance_config_t *detector_config, distance_preset_config_t preset);
bool initialize_detector_resources(distance_detector_resources_t *resources);
bool do_sensor_calibration(acc_sensor_t *sensor,
acc_cal_result_t *sensor_cal_result,
void *buffer,
uint32_t buffer_size);
bool do_full_detector_calibration(distance_detector_resources_t *resources,
const acc_cal_result_t *sensor_cal_result);
bool do_detector_calibration_update(distance_detector_resources_t *resources,
const acc_cal_result_t *sensor_cal_result);
bool do_detector_get_next(distance_detector_resources_t *resources,
const acc_cal_result_t *sensor_cal_result,
acc_detector_distance_result_t *result);
void print_distance_result(const acc_detector_distance_result_t *result);

};

224 changes: 222 additions & 2 deletions range_sensor/Core/Src/range_sensor.cpp
View file
Original file line number Diff line number Diff line change
@@ -5,8 +5,228 @@
* Author: praka
*/

void read_radar (void)
static void Rangefinder::set_config(acc_detector_distance_config_t *detector_config, distance_preset_config_t preset)
{
HAL_SPI_Receive(hspi1, );
// Add configuration of the detector here
switch (preset)
{
case DISTANCE_PRESET_CONFIG_NONE:
// Add configuration of the detector here
break;

case DISTANCE_PRESET_CONFIG_BALANCED:
acc_detector_distance_config_start_set(detector_config, 0.25f);
acc_detector_distance_config_end_set(detector_config, 3.0f);
acc_detector_distance_config_max_step_length_set(detector_config, 0U);
acc_detector_distance_config_max_profile_set(detector_config, ACC_CONFIG_PROFILE_5);
acc_detector_distance_config_reflector_shape_set(detector_config, ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_GENERIC);
acc_detector_distance_config_peak_sorting_set(detector_config, ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST);
acc_detector_distance_config_threshold_method_set(detector_config, ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR);
acc_detector_distance_config_threshold_sensitivity_set(detector_config, 0.5f);
acc_detector_distance_config_signal_quality_set(detector_config, 15.0f);
acc_detector_distance_config_close_range_leakage_cancellation_set(detector_config, false);
break;

case DISTANCE_PRESET_CONFIG_HIGH_ACCURACY:
acc_detector_distance_config_start_set(detector_config, 0.25f);
acc_detector_distance_config_end_set(detector_config, 3.0f);
acc_detector_distance_config_max_step_length_set(detector_config, 2U);
acc_detector_distance_config_max_profile_set(detector_config, ACC_CONFIG_PROFILE_3);
acc_detector_distance_config_reflector_shape_set(detector_config, ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_GENERIC);
acc_detector_distance_config_peak_sorting_set(detector_config, ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST);
acc_detector_distance_config_threshold_method_set(detector_config, ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR);
acc_detector_distance_config_threshold_sensitivity_set(detector_config, 0.5f);
acc_detector_distance_config_signal_quality_set(detector_config, 20.0f);
acc_detector_distance_config_close_range_leakage_cancellation_set(detector_config, false);
break;
}
}


static bool Rangefinder::initialize_detector_resources(distance_detector_resources_t *resources)
{
resources->handle = acc_detector_distance_create(resources->config);
if (resources->handle == NULL)
{
printf("acc_detector_distance_create() failed\n");
return false;
}

if (!acc_detector_distance_get_sizes(resources->handle, &(resources->buffer_size), &(resources->detector_cal_result_static_size)))
{
printf("acc_detector_distance_get_sizes() failed\n");
return false;
}

resources->buffer = acc_integration_mem_alloc(resources->buffer_size);
if (resources->buffer == NULL)
{
printf("sensor buffer allocation failed\n");
return false;
}

resources->detector_cal_result_static = acc_integration_mem_alloc(resources->detector_cal_result_static_size);
if (resources->detector_cal_result_static == NULL)
{
printf("calibration buffer allocation failed\n");
return false;
}

return true;
}


static bool Rangefinder::do_sensor_calibration(acc_sensor_t *sensor,
acc_cal_result_t *sensor_cal_result,
void *buffer,
uint32_t buffer_size)
{
bool status = false;
bool cal_complete = false;
const uint16_t calibration_retries = 1U;

// Random disturbances may cause the calibration to fail. At failure, retry at least once.
for (uint16_t i = 0; !status && (i <= calibration_retries); i++)
{
// Reset sensor before calibration by disabling/enabling it
acc_hal_integration_sensor_disable(SENSOR_ID);
acc_hal_integration_sensor_enable(SENSOR_ID);

do
{
status = acc_sensor_calibrate(sensor, &cal_complete, sensor_cal_result, buffer, buffer_size);

if (status && !cal_complete)
{
status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
}
} while (status && !cal_complete);
}

if (status)
{
/* Reset sensor after calibration by disabling/enabling it */
acc_hal_integration_sensor_disable(SENSOR_ID);
acc_hal_integration_sensor_enable(SENSOR_ID);
}

return status;
}


static bool Rangefinder::do_full_detector_calibration(distance_detector_resources_t *resources,
const acc_cal_result_t *sensor_cal_result)
{
bool done = false;
bool status;

do
{
status = acc_detector_distance_calibrate(resources->sensor,
resources->handle,
sensor_cal_result,
resources->buffer,
resources->buffer_size,
resources->detector_cal_result_static,
resources->detector_cal_result_static_size,
&resources->detector_cal_result_dynamic,
&done);

if (status && !done)
{
status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
}
} while (status && !done);

return status;
}


static bool Rangefinder::do_detector_calibration_update(distance_detector_resources_t *resources,
const acc_cal_result_t *sensor_cal_result)
{
bool done = false;
bool status;

do
{
status = acc_detector_distance_update_calibration(resources->sensor,
resources->handle,
sensor_cal_result,
resources->buffer,
resources->buffer_size,
&resources->detector_cal_result_dynamic,
&done);

if (status && !done)
{
status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
}
} while (status && !done);

return status;
}


static bool Rangefinder::do_detector_get_next(distance_detector_resources_t *resources,
const acc_cal_result_t *sensor_cal_result,
acc_detector_distance_result_t *result)
{
bool result_available = false;

do
{
if (!acc_detector_distance_prepare(resources->handle, resources->config, resources->sensor, sensor_cal_result, resources->buffer,
resources->buffer_size))
{
printf("acc_detector_distance_prepare() failed\n");
return false;
}

if (!acc_sensor_measure(resources->sensor))
{
printf("acc_sensor_measure() failed\n");
return false;
}

if (!acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS))
{
printf("Sensor interrupt timeout\n");
return false;
}

if (!acc_sensor_read(resources->sensor, resources->buffer, resources->buffer_size))
{
printf("acc_sensor_read() failed\n");
return false;
}

if (!acc_detector_distance_process(resources->handle, resources->buffer, resources->detector_cal_result_static,
&resources->detector_cal_result_dynamic,
&result_available, result))
{
printf("acc_detector_distance_process() failed\n");
return false;
}
} while (!result_available);

return true;
}


static void Rangefinder::print_distance_result(const acc_detector_distance_result_t *result)
{
printf("%d detected distances", result->num_distances);
if (result->num_distances > 0)
{
printf(": ");

for (uint8_t i = 0; i < result->num_distances; i++)
{
printf("%" PRIfloat " m ", ACC_LOG_FLOAT_TO_INTEGER(result->distances[i]));
}
}

printf("\n");
}