419 lines
12 KiB
C
419 lines
12 KiB
C
// Copyright (c) Acconeer AB, 2020-2022
|
|
// All rights reserved
|
|
// This file is subject to the terms and conditions defined in the file
|
|
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
|
|
// of this source code package.
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include "sys_app.h"
|
|
#include "acc_hal_definitions.h"
|
|
#include "acc_hal_integration.h"
|
|
#include "acc_integration.h"
|
|
#include "acc_rss.h"
|
|
#include "acc_service.h"
|
|
#include "acc_service_envelope.h"
|
|
#include "acc_version.h"
|
|
|
|
|
|
// Default values for this reference application
|
|
// ---------------------------------------------
|
|
|
|
// Service configuration settings
|
|
#define SENSOR_ID 1
|
|
#define RANGE_START_M 0.12f
|
|
#define RANGE_LENGTH_M 0.50f
|
|
#define SERVICE_DOWNSAMPLING 2
|
|
#define SERVICE_HWAAS 20
|
|
#define RUNNING_AVERAGE_FACTOR 0.0f
|
|
#define POWER_SAVE_MODE ACC_POWER_SAVE_MODE_OFF
|
|
|
|
// The expected background level for the envelope service
|
|
// This parameter is not expected to change as long as noise level normalization is active
|
|
#define ENVELOPE_BACKGROUND_LEVEL 100
|
|
|
|
// Parameters for direct leakage subtraction
|
|
// To be set from inspection of the worst case direct leakage
|
|
// LEAKAGE_SAMPLE_POSITION_M and LEAKAGE_END_POSITION_M must be within
|
|
// the measurement range [RANGE_START_M, RANGE_START_M + RANGE_LENGTH_M]
|
|
// and LEAKAGE_END_POSITION_M must be beyond LEAKAGE_SAMPLE_POSITION_M
|
|
#define LEAKAGE_SAMPLE_POSITION_M 0.15f
|
|
#define LEAKAGE_END_POSITION_M 0.30f
|
|
#define MAX_LEAK_AMPLITUDE 2000
|
|
|
|
// The number of observations in the parking detection queue
|
|
#define DETECTION_OBSERVATION_COUNT 3
|
|
|
|
// The minimal weight for each observation in the parking detection queue for
|
|
// detection of a parked car
|
|
#define DETECTION_WEIGHT_THRESHOLD 5.0f
|
|
|
|
// Parameters for exclusion of transient reflections from people and items that are near the
|
|
// sensor for short durations
|
|
#define DETECTION_WEIGHT_RATIO_LIMIT 3.0f
|
|
#define DETECTION_DISPLACEMENT_LIMIT 0.1f
|
|
|
|
// The time duration between two consecutive sweeps
|
|
#define DETECTOR_SWEEP_PERIOD_S 10.0f
|
|
|
|
// Minimal envelope service runtime before sensor recalibration due to a data quality warning.
|
|
// A sensor calibration is costly in terms of power consumption relative to the sweeps
|
|
#define SERVICE_RUNTIME_MIN_S 900.0f
|
|
|
|
// Maximal envelope service active time before the service is destroyed and recreated
|
|
// to renew the noise level normalization. The sensor output amplitude may change notably due
|
|
// to temperature changes before a data quality warning is triggered. This type of refresh
|
|
// is inexpensive with respect to power consumption.
|
|
#define SERVICE_UPTIME_MAX_S 900.0f
|
|
|
|
|
|
typedef struct
|
|
{
|
|
float weight;
|
|
float distance;
|
|
} sweep_observable_t;
|
|
|
|
|
|
/**
|
|
* Recreate the service to renew the noise noise level normalization
|
|
*
|
|
* @param configuration The configuration to recreate
|
|
* @param handle The handle of the service to recreate
|
|
* @return True, if reactivation was successful
|
|
*/
|
|
static bool service_recreate(const acc_service_configuration_t configuration, acc_service_handle_t *handle);
|
|
|
|
|
|
/**
|
|
* Calibrate the sensor
|
|
*
|
|
* @return True, if sensor calibration was successful
|
|
*/
|
|
static bool sensor_calibration(void);
|
|
|
|
|
|
/**
|
|
* Configure the service to the specified configuration
|
|
*
|
|
* @param configuration The service configuration to configure with application settings
|
|
*/
|
|
static void configure_service(acc_service_configuration_t configuration);
|
|
|
|
|
|
/**
|
|
* Exectute the parking detection
|
|
*
|
|
* @param metadata Service metadata
|
|
* @param leak_sample_index Index where to sample leakage
|
|
* @param leak_end_index Index where leakage is assumed to end
|
|
* @param observations Observation history
|
|
* @param observation_count The number of observations in the history
|
|
* @param data Service data
|
|
* @return True, if a car is detected
|
|
*/
|
|
static bool parking_detection(const acc_service_envelope_metadata_t *metadata, uint16_t leak_sample_index,
|
|
uint16_t leak_end_index, sweep_observable_t *observations, uint16_t *observation_count,
|
|
const uint16_t *data);
|
|
|
|
|
|
int acc_ref_app_parking(int argc, char *argv[]);
|
|
|
|
|
|
int acc_ref_app_parking(int argc, char *argv[])
|
|
{
|
|
(void)argc;
|
|
(void)argv;
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Acconeer software version %s\n", acc_version_get());
|
|
|
|
const acc_hal_t *hal = acc_hal_integration_get_implementation();
|
|
|
|
if (!acc_rss_activate(hal))
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to activate RSS\n");
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
acc_service_configuration_t configuration = acc_service_envelope_configuration_create();
|
|
|
|
if (configuration == NULL)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to create service configuration\n");
|
|
acc_rss_deactivate();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
if (!sensor_calibration())
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to calibrate sensor\n");
|
|
acc_service_envelope_configuration_destroy(&configuration);
|
|
acc_rss_deactivate();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
configure_service(configuration);
|
|
|
|
acc_service_handle_t handle = acc_service_create(configuration);
|
|
|
|
if (handle == NULL)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to create service handle\n");
|
|
acc_service_envelope_configuration_destroy(&configuration);
|
|
acc_rss_deactivate();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
acc_service_envelope_metadata_t metadata;
|
|
acc_service_envelope_get_metadata(handle, &metadata);
|
|
|
|
uint16_t leak_sample_index = (uint16_t)(((LEAKAGE_SAMPLE_POSITION_M - metadata.start_m) / metadata.step_length_m) + 0.5f);
|
|
uint16_t leak_end_index = (uint16_t)(((LEAKAGE_END_POSITION_M - metadata.start_m) / metadata.step_length_m) + 0.5f);
|
|
bool valid_leak_setup = (leak_sample_index < leak_end_index && leak_sample_index < metadata.data_length);
|
|
|
|
uint16_t *data = NULL;
|
|
acc_service_envelope_result_info_t result_info;
|
|
sweep_observable_t observations[DETECTION_OBSERVATION_COUNT];
|
|
uint16_t observation_count = 0;
|
|
uint32_t last_update_ms = 0;
|
|
uint32_t last_activate_ms = hal->os.gettime();
|
|
uint32_t last_calibration_ms = hal->os.gettime();
|
|
uint16_t sweep_index = 0;
|
|
char buf[64]="";
|
|
|
|
bool status = true;
|
|
|
|
if (!valid_leak_setup)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Parameters are not valid\n");
|
|
status = false;
|
|
}
|
|
else
|
|
{
|
|
status = acc_service_activate(handle);
|
|
}
|
|
|
|
while (status)
|
|
{
|
|
if (hal->os.gettime() - last_activate_ms > SERVICE_UPTIME_MAX_S * 1000)
|
|
{
|
|
status = service_recreate(configuration, &handle);
|
|
last_activate_ms = hal->os.gettime();
|
|
}
|
|
|
|
if (status)
|
|
{
|
|
while (last_update_ms != 0 && hal->os.gettime() - last_update_ms < DETECTOR_SWEEP_PERIOD_S * 1000)
|
|
{
|
|
acc_integration_sleep_ms((DETECTOR_SWEEP_PERIOD_S * 1000) - (hal->os.gettime() - last_update_ms));
|
|
}
|
|
|
|
status = acc_service_envelope_get_next_by_reference(handle, &data, &result_info);
|
|
last_update_ms = hal->os.gettime();
|
|
}
|
|
|
|
if (status && result_info.data_quality_warning &&
|
|
hal->os.gettime() - last_calibration_ms > SERVICE_RUNTIME_MIN_S * 1000)
|
|
{
|
|
status = acc_service_deactivate(handle);
|
|
|
|
if (status)
|
|
{
|
|
acc_service_destroy(&handle);
|
|
|
|
status = sensor_calibration();
|
|
}
|
|
|
|
if (status)
|
|
{
|
|
handle = acc_service_create(configuration);
|
|
|
|
status = handle != NULL;
|
|
}
|
|
|
|
if (status)
|
|
{
|
|
status = acc_service_activate(handle);
|
|
}
|
|
|
|
last_calibration_ms = hal->os.gettime();
|
|
|
|
if (status)
|
|
{
|
|
status = acc_service_envelope_get_next_by_reference(handle, &data, &result_info);
|
|
last_update_ms = hal->os.gettime();
|
|
}
|
|
}
|
|
|
|
if (status)
|
|
{
|
|
bool detection = parking_detection(&metadata, leak_sample_index, leak_end_index, observations, &observation_count, data);
|
|
|
|
if (sweep_index < DETECTION_OBSERVATION_COUNT - 1)
|
|
{
|
|
sprintf(buf,"%" PRIu16 ": No result\n", sweep_index);
|
|
APP_LOG(TS_OFF, VLEVEL_M,"%s",buf);
|
|
}
|
|
else
|
|
{
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"%" PRIu16 ": %s\n", sweep_index, detection ? "Car" : "No car");
|
|
sprintf(buf,"%" PRIu16 ": %s\n", sweep_index, detection ? "Car" : "No car");
|
|
APP_LOG(TS_OFF, VLEVEL_M,"%s",buf);
|
|
}
|
|
|
|
sweep_index++;
|
|
}
|
|
}
|
|
|
|
acc_service_envelope_configuration_destroy(&configuration);
|
|
acc_service_deactivate(handle);
|
|
acc_service_destroy(&handle);
|
|
acc_rss_deactivate();
|
|
|
|
return status ? EXIT_SUCCESS : EXIT_FAILURE;
|
|
}
|
|
|
|
|
|
bool service_recreate(const acc_service_configuration_t configuration, acc_service_handle_t *handle)
|
|
{
|
|
if (!acc_service_deactivate(*handle))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
acc_service_destroy(handle);
|
|
|
|
*handle = acc_service_create(configuration);
|
|
|
|
if (*handle == NULL)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!acc_service_activate(*handle))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool sensor_calibration(void)
|
|
{
|
|
acc_calibration_context_t calibration_context;
|
|
|
|
if (!acc_rss_calibration_context_get(SENSOR_ID, &calibration_context))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!acc_rss_calibration_context_forced_set(SENSOR_ID, &calibration_context))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
void configure_service(acc_service_configuration_t configuration)
|
|
{
|
|
acc_service_sensor_set(configuration, SENSOR_ID);
|
|
acc_service_requested_start_set(configuration, RANGE_START_M);
|
|
acc_service_requested_length_set(configuration, RANGE_LENGTH_M);
|
|
acc_service_envelope_downsampling_factor_set(configuration, SERVICE_DOWNSAMPLING);
|
|
acc_service_hw_accelerated_average_samples_set(configuration, SERVICE_HWAAS);
|
|
acc_service_envelope_running_average_factor_set(configuration, RUNNING_AVERAGE_FACTOR);
|
|
acc_service_power_save_mode_set(configuration, POWER_SAVE_MODE);
|
|
}
|
|
|
|
|
|
bool parking_detection(const acc_service_envelope_metadata_t *metadata, uint16_t leak_sample_index,
|
|
uint16_t leak_end_index, sweep_observable_t *observations, uint16_t *observation_count,
|
|
const uint16_t *data)
|
|
{
|
|
float weight_sum = 0.0f;
|
|
float weight_sum_r = 0.0f;
|
|
float leak_start = 0.0f;
|
|
uint16_t leak_amplitude = MAX_LEAK_AMPLITUDE < data[leak_sample_index] ? MAX_LEAK_AMPLITUDE : data[leak_sample_index];
|
|
uint16_t a_leak = leak_amplitude < ENVELOPE_BACKGROUND_LEVEL ? 0 : leak_amplitude - ENVELOPE_BACKGROUND_LEVEL;
|
|
float leak_step = ((float)(a_leak) / (leak_end_index - leak_sample_index));
|
|
|
|
leak_start = leak_end_index * leak_step + ENVELOPE_BACKGROUND_LEVEL;
|
|
|
|
for (uint16_t i = 0; i < metadata->data_length; i++)
|
|
{
|
|
float r = metadata->start_m + i * metadata->step_length_m;
|
|
float bg = 0.0f;
|
|
if (i <= leak_end_index)
|
|
{
|
|
bg = leak_start - i * leak_step;
|
|
}
|
|
else
|
|
{
|
|
bg = ENVELOPE_BACKGROUND_LEVEL;
|
|
}
|
|
|
|
float sweep_above_bg = data[i] - bg;
|
|
sweep_above_bg = sweep_above_bg > 0.0f ? sweep_above_bg : 0.0f;
|
|
|
|
float weight = sweep_above_bg / ENVELOPE_BACKGROUND_LEVEL;
|
|
weight = weight < 1.0f ? weight : 1.0f;
|
|
weight = weight * sweep_above_bg * r;
|
|
|
|
weight_sum += weight;
|
|
weight_sum_r += weight * r;
|
|
}
|
|
|
|
if (*observation_count == DETECTION_OBSERVATION_COUNT)
|
|
{
|
|
for (uint16_t i = 1; i < DETECTION_OBSERVATION_COUNT; i++)
|
|
{
|
|
observations[i - 1].weight = observations[i].weight;
|
|
observations[i - 1].distance = observations[i].distance;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
(*observation_count)++;
|
|
}
|
|
|
|
observations[*observation_count - 1].weight = weight_sum / metadata->data_length;
|
|
observations[*observation_count - 1].distance = weight_sum_r / weight_sum;
|
|
|
|
float weight_min = __FLT_MAX__;
|
|
float weight_max = 0.0f;
|
|
float distance_min = __FLT_MAX__;
|
|
float distance_max = 0.0f;
|
|
|
|
for (uint16_t i = 0; i < *observation_count; i++)
|
|
{
|
|
if (weight_min > observations[i].weight)
|
|
{
|
|
weight_min = observations[i].weight;
|
|
}
|
|
|
|
if (weight_max < observations[i].weight)
|
|
{
|
|
weight_max = observations[i].weight;
|
|
}
|
|
|
|
if (distance_min > observations[i].distance)
|
|
{
|
|
distance_min = observations[i].distance;
|
|
}
|
|
|
|
if (distance_max < observations[i].distance)
|
|
{
|
|
distance_max = observations[i].distance;
|
|
}
|
|
}
|
|
|
|
bool detection = *observation_count == DETECTION_OBSERVATION_COUNT &&
|
|
weight_min >= DETECTION_WEIGHT_THRESHOLD &&
|
|
weight_max / weight_min <= DETECTION_WEIGHT_RATIO_LIMIT &&
|
|
distance_max - distance_min <= DETECTION_DISPLACEMENT_LIMIT;
|
|
|
|
return detection;
|
|
}
|