void Install_clock(
rtems_isr_entry clock_isr
)
{
/*
* Initialize the clock tick device driver variables
*/
Clock_driver_ticks = 0;
Clock_isrs = rtems_configuration_get_milliseconds_per_tick();
mips_timer_rate = rtems_configuration_get_microseconds_per_tick() *
bsp_clicks_per_microsecond;
/*
* Hardware specific initialize goes here
*/
/* Set up USC heartbeat timer to generate interrupts */
disable_hbi(); /* Disable heartbeat interrupt in USC */
/* Install interrupt handler */
rtems_interrupt_handler_install(
CLOCK_VECTOR,
"clock",
0,
USC_isr,
NULL
);
init_hbt(); /* Initialize heartbeat timer */
reset_wdt(); /* Reset watchdog timer */
enable_wdi(); /* Enable watchdog interrupt in USC */
enable_hbi(); /* Enable heartbeat interrupt in USC */
/* Enable USC interrupt in MIPS processor */
mips_enable_in_interrupt_mask(CLOCK_VECTOR_MASK);
/*
* Schedule the clock cleanup routine to execute if the application exits.
*/
atexit( Clock_exit );
}
rtems_isr Clock_isr(
rtems_vector_number vector
)
{
/*
* bump the number of clock driver ticks since initialization
*
* determine if it is time to announce the passing of tick as configured
* to RTEMS through the rtems_clock_tick directive
*
* perform any timer dependent tasks
*/
reset_wdt(); /* Reset hardware watchdog timer */
Clock_driver_ticks += 1;
rtems_clock_tick();
}
int main()
{
int Status;
init_axi_uart();
init_axi_gpio();
init_spi();
init_pendulum_plant();
init_fifo_queues();
init_wdt();
assert_trigger(PRODUCTION);
set_set_point(0);
Status = init_interrupt_system();
if (Status != XST_SUCCESS) {
xil_printf("Failed intr setup\r\n");
return XST_FAILURE;
}
start_ioi();
static bool startTAIGA = false;
static bool assertTrigger = false;
while(true){
if(!startTAIGA && read_btn(BTN0)){
startTAIGA = true;
start_wdt();
}
assert_trigger(startTAIGA & (check_wdt() | assertTrigger));
supervisor_update_set_point();
if(check_control_cycle()){
set_debug(DEBUG2, true);
reset_control_cycle();
if((get_alphaR() >= 0 ? get_alphaR():-get_alphaR()) < (20.*pi/180))
calculateKalmanControlSignal(get_plant_state_instance());
set_debug(DEBUG2, false);
supervisor_send_state_vector(get_plant_state_instance()->xhat);
set_debug(DEBUG2, true);
if(startTAIGA){
reset_wdt();
set_debug(DEBUG4, true);
if((get_alphaR() >= 0 ? get_alphaR():-get_alphaR()) < (20.*pi/180)){
if(trivial_trigger_mechanism(get_plant_state_instance())){
assertTrigger = true;
set_debug(DEBUG3, true);
}
else set_debug(DEBUG3, false);
}
set_debug(DEBUG4, false);
}
supervisor_send_tail(get_plant_state_instance()->u, startTAIGA, assertTrigger, check_wdt());
set_debug(DEBUG2, false);
}
}
return XST_SUCCESS;
}
/**
* @brief Timer routine for fall & step detection.
*/
void process_step_count(void * p_context)
{
//timer_stop(m_app_timer_id);
reset_wdt();
nrf_gpio_pin_toggle(LED_RESETTING);
if (NRF_NVMC->READY == NVMC_READY_READY_Busy)
{
// Do not interfere if flash write is ongoing.
return;
}
if (SaveSnapshot == 1)
{
nrf_gpio_pin_set(LED_RESETTING);
return;
}
if (ReadBigSnapshot == 1)
return;
// store snapshot
readSensorsAndStore();
//process activity levels
updateAverages();
curr_sv = latestResultant();
if (BLESessionActive == 1)
return;
if (curr_sv < getPerso()->freefall_threshold) {
zero_g_detected = true;
}
// if ( checkOrientation() == ORIENTATION_STANDING){
// if (curr_sv < getPerso()->freefall_threshold){
// zero_g_detected = true;
// }
// }
// else if (checkOrientation() == ORIENTATION_NOT_STANDING)
// {
// if (curr_sv < NOT_STANDING_FREEFALL_THRESHOLD_DEFAULT){
// zero_g_detected = true;
// }
// }
else {
zero_g_detected = false;
}
//zero_g_detected = fd_check_zero_g();
if (zero_g_detected == true)
{
nrf_gpio_pin_write(LED_OTHER,1);
// if (count == 0)
accel_get_angles(&xAngleBeforeImpact, &yAngleBeforeImpact);
count = getPerso()->fall_detection_window;
}
if (count > 0)
{
count--;
fall_detected = fd_check_for_impact(curr_sv);
if (fall_detected == 0xFF)
{
accel_get_angles(&xAngleAfterImpact, &yAngleAfterImpact);
// if (abs(xAngleAfterImpact - xAngleBeforeImpact) >= getPerso()->x_angle_threshold &&
// abs(yAngleAfterImpact - yAngleBeforeImpact) >= getPerso()->y_angle_threshold)
{
ClearDataBuffer = ClearSnapshots = SetTimestamp = 0;
// backupRawData();
do_post_processing = true;
emergencyCall = 1;
zero_g_detected = false;
fall_detected = 0;
count = 0;
}
}
}
if (count == 0)
nrf_gpio_pin_write(LED_OTHER,0);
/*
else
{
// detect if step occurred
if (detect_step(xRaw,yRaw,zRaw)==1)
{
nSteps++;
}
}
*/
}
