316 lines
9.0 KiB
C
316 lines
9.0 KiB
C
|
|
||
|
|
int sts_presence_rss_fall_rise_detection(void)
|
||
|
|
{
|
||
|
|
const acc_hal_t *hal = acc_hal_integration_get_implementation();
|
||
|
|
|
||
|
|
if (!acc_rss_activate(hal))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to activate RSS\n");
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_rss_override_sensor_id_check_at_creation(true);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_t presence_configuration = acc_detector_presence_configuration_create();
|
||
|
|
if (presence_configuration == NULL)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to create configuration\n");
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (sts_rss_config_updated_flag)
|
||
|
|
{
|
||
|
|
case STS_RSS_CONFIG_DEFAULT:
|
||
|
|
set_default_configuration(presence_configuration);
|
||
|
|
break;
|
||
|
|
case STS_RSS_CONFIG_SIMPLE:
|
||
|
|
sts_rss_set_current_configuration_simple(presence_configuration);
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_L,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n");
|
||
|
|
|
||
|
|
break;
|
||
|
|
case STS_RSS_CONFIG_FULL:
|
||
|
|
sts_rss_set_current_configuration_full(presence_configuration);
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_L,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n");
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT; //update finished, set to 0
|
||
|
|
|
||
|
|
acc_detector_presence_handle_t handle = acc_detector_presence_create(presence_configuration);
|
||
|
|
if (handle == NULL)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to create detector\n");
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ******** First Half detection of fall down and rise up
|
||
|
|
if (!acc_detector_presence_activate(handle))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H, "Failed to activate detector \n");
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool success = true;
|
||
|
|
const int iterations = (DEFAULT_UPDATE_RATE_PRESENCE+1);
|
||
|
|
acc_detector_presence_result_t result;
|
||
|
|
uint8_t average_result = 0;
|
||
|
|
float average_distance =0.0f;
|
||
|
|
float average_score =0.0f;
|
||
|
|
for (int i = 0; i < (iterations/2); i++)
|
||
|
|
{
|
||
|
|
success = acc_detector_presence_get_next(handle, &result);
|
||
|
|
if (!success)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"acc_detector_presence_get_next() failed\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
print_result(result);
|
||
|
|
//if (!result.data_saturated)
|
||
|
|
{
|
||
|
|
if (result.presence_detected)
|
||
|
|
{
|
||
|
|
average_result++;
|
||
|
|
average_distance += result.presence_distance;
|
||
|
|
average_score += result.presence_score;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (sts_presence_fall_detection == 1)
|
||
|
|
{
|
||
|
|
sts_motion_dataset[motion_count].presence_distance = result.presence_distance;
|
||
|
|
sts_motion_dataset[motion_count].presence_score = result.presence_score;
|
||
|
|
|
||
|
|
if (motion_count ++ == DEFAULT_MOTION_DATASET_LEN)
|
||
|
|
{
|
||
|
|
STS_YunhornCheckStandardDeviation();
|
||
|
|
motion_count = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // DEFAULT_UPDATE_RATE);
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_detector_presence_deactivate(handle);
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_L,"First Half Presence Detection, Motion Count = %u \r\n", (int)motion_count);
|
||
|
|
|
||
|
|
// ******** Second Half detection of fall down and rise up
|
||
|
|
|
||
|
|
set_default_fall_rise_configuration(presence_configuration);
|
||
|
|
|
||
|
|
if (!acc_detector_presence_reconfigure(&handle, presence_configuration))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to reconfigure detector\n");
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
if (!acc_detector_presence_activate(handle))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H, "Failed to activate detector \n");
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
|
||
|
|
for (int i = 0; i < (iterations/2); i++)
|
||
|
|
{
|
||
|
|
success = acc_detector_presence_get_next(handle, &result);
|
||
|
|
if (!success)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"acc_detector_presence_get_next() failed\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
print_result(result);
|
||
|
|
//if (!result.data_saturated)
|
||
|
|
{
|
||
|
|
if (result.presence_detected)
|
||
|
|
{
|
||
|
|
average_result++;
|
||
|
|
average_distance += result.presence_distance;
|
||
|
|
average_score += result.presence_score;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (sts_presence_fall_detection == 1)
|
||
|
|
{
|
||
|
|
sts_motion_dataset[motion_count].presence_distance = result.presence_distance;
|
||
|
|
sts_motion_dataset[motion_count].presence_score = result.presence_score;
|
||
|
|
|
||
|
|
if (motion_count ++ == DEFAULT_MOTION_DATASET_LEN)
|
||
|
|
{
|
||
|
|
STS_YunhornCheckStandardDeviation();
|
||
|
|
motion_count = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // DEFAULT_UPDATE_RATE);
|
||
|
|
}
|
||
|
|
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_L,"Second Half, Fall Rise Detection, Motion Count = %u \r\n", (int)motion_count);
|
||
|
|
|
||
|
|
sts_rss_result = (average_result > 3)? 1: 0;
|
||
|
|
average_distance = (1000.0f*average_distance)/average_result; // in meters
|
||
|
|
average_score = (1000.0f*average_score)/average_result;
|
||
|
|
sts_presence_rss_distance = average_distance;
|
||
|
|
sts_presence_rss_score = average_score;
|
||
|
|
if (average_score !=0) //if (sts_rss_result)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"\r\n######## Motion: %u Distance=%u mm, Score=%u Average_result=%u out of %u \r\n",
|
||
|
|
(uint8_t)sts_rss_result,(int) average_distance, (int)(average_score), (int)average_result, (int)iterations);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
bool deactivated = acc_detector_presence_deactivate(handle);
|
||
|
|
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
|
||
|
|
acc_rss_deactivate();
|
||
|
|
|
||
|
|
if (deactivated && success)
|
||
|
|
{
|
||
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"Application finished OK\n");
|
||
|
|
return EXIT_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
int sts_presence_rss_presence_detection(void)
|
||
|
|
{
|
||
|
|
const acc_hal_t *hal = acc_hal_integration_get_implementation();
|
||
|
|
|
||
|
|
if (!acc_rss_activate(hal))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to activate RSS\n");
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_rss_override_sensor_id_check_at_creation(true);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_t presence_configuration = acc_detector_presence_configuration_create();
|
||
|
|
if (presence_configuration == NULL)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to create configuration\n");
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (sts_rss_config_updated_flag)
|
||
|
|
{
|
||
|
|
case STS_RSS_CONFIG_DEFAULT:
|
||
|
|
set_default_configuration(presence_configuration);
|
||
|
|
break;
|
||
|
|
case STS_RSS_CONFIG_SIMPLE:
|
||
|
|
sts_rss_set_current_configuration_simple(presence_configuration);
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_L,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n");
|
||
|
|
|
||
|
|
break;
|
||
|
|
case STS_RSS_CONFIG_FULL:
|
||
|
|
sts_rss_set_current_configuration_full(presence_configuration);
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_L,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n");
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT; //update finished, set to 0
|
||
|
|
|
||
|
|
acc_detector_presence_handle_t handle = acc_detector_presence_create(presence_configuration);
|
||
|
|
if (handle == NULL)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to create detector\n");
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
|
||
|
|
if (!acc_detector_presence_activate(handle))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"Failed to activate detector\n");
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool success = true;
|
||
|
|
const int iterations = DEFAULT_UPDATE_RATE_PRESENCE;
|
||
|
|
acc_detector_presence_result_t result;
|
||
|
|
uint8_t average_result = 0;
|
||
|
|
float average_distance =0.0f;
|
||
|
|
float average_score =0.0f;
|
||
|
|
for (int i = 0; i < iterations; i++)
|
||
|
|
{
|
||
|
|
success = acc_detector_presence_get_next(handle, &result);
|
||
|
|
if (!success)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"acc_detector_presence_get_next() failed\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
print_result(result);
|
||
|
|
//if (!result.data_saturated)
|
||
|
|
{
|
||
|
|
if (result.presence_detected)
|
||
|
|
{
|
||
|
|
average_result++;
|
||
|
|
average_distance += result.presence_distance;
|
||
|
|
average_score += result.presence_score;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (sts_presence_fall_detection == 1)
|
||
|
|
{
|
||
|
|
sts_motion_dataset[motion_count].presence_distance = result.presence_distance;
|
||
|
|
sts_motion_dataset[motion_count].presence_score = result.presence_score;
|
||
|
|
|
||
|
|
if (motion_count ++ == DEFAULT_MOTION_DATASET_LEN)
|
||
|
|
{
|
||
|
|
STS_YunhornCheckStandardDeviation();
|
||
|
|
motion_count = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // DEFAULT_UPDATE_RATE);
|
||
|
|
}
|
||
|
|
|
||
|
|
sts_rss_result = (average_result > 3)? 1: 0;
|
||
|
|
average_distance = (1000.0f*average_distance)/average_result; // in meters
|
||
|
|
average_score = (1000.0f*average_score)/average_result;
|
||
|
|
sts_presence_rss_distance = average_distance;
|
||
|
|
sts_presence_rss_score = average_score;
|
||
|
|
if (average_score !=0) //if (sts_rss_result)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_H,"\r\n######## Motion: %u Distance=%u mm, Score=%u Average_result=%u out of %u \r\n",
|
||
|
|
(uint8_t)sts_rss_result,(int) average_distance, (int)(average_score), (int)average_result, (int)iterations);
|
||
|
|
}
|
||
|
|
|
||
|
|
bool deactivated = acc_detector_presence_deactivate(handle);
|
||
|
|
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
|
||
|
|
acc_rss_deactivate();
|
||
|
|
|
||
|
|
if (deactivated && success)
|
||
|
|
{
|
||
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"Application finished OK\n");
|
||
|
|
return EXIT_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|