int main( void )
{
//initMemoryPool();
//setup hardware
updateActiveLoggerConfig();
int success = setupHardware();
if (! success) fatalError(FATAL_ERROR_HARDWARE);
init_logger_data();
initMessaging();
xTaskCreate( vUSBCDCTask, ( signed portCHAR * ) "USB", mainUSB_TASK_STACK, NULL, mainUSB_PRIORITY, NULL );
xTaskCreate( onUSBCommTask, ( signed portCHAR * ) "OnUSBComm", mainUSB_COMM_STACK, NULL, USB_COMM_TASK_PRIORITY, NULL );
#ifdef LUA_ENABLED
startLuaTask();
#endif
createFileWriterTask();
createLoggerTaskEx();
createGPIOTasks();
createConnectivityTask();
startGPSTask();
/* Start the scheduler.
NOTE : Tasks run in system mode and the scheduler runs in Supervisor mode.
The processor MUST be in supervisor mode when vTaskStartScheduler is
called. The demo applications included in the FreeRTOS.org download switch
to supervisor mode prior to main being called. If you are not using one of
these demo application projects then ensure Supervisor mode is used here. */
vTaskStartScheduler();
fatalError(FATAL_ERROR_SCHEDULER);
return 0;
}
int main(void)
{
// TODO: insert code here
//Incialización del Hardware
setupHardware();
//Trace del RTOS
//traceSTART();
//Creacion del semaforo
cola = xQueueCreate(10, sizeof(long));
//Creación de las tareas
xTaskCreate( taskLED,
(const signed char * const)"taskLED",
configMINIMAL_STACK_SIZE,
NULL,
tskIDLE_PRIORITY+1,
NULL );
//Inicio el Scheduler
vTaskStartScheduler();
while(1);
}
int main(void) {
setupHardware();
xTaskCreate(taskBlink, "taskBlink", configMINIMAL_STACK_SIZE, 0, tskIDLE_PRIORITY+1, 0);
vTaskStartScheduler();
while (TRUE);
return 0;
}
int main(void )
{
setupHardware();
init_CIM_frame();
old_PIND=PIND;
old_PINB=PINB;
sei(); // enable global interrupts
while (1)
{
parse_CIM_protocol();
if (send_ADCFrame_now) // this is updated in the timer ISR !!
{
send_ADCFrame_now=0;
autoreply_num++;
if (autoreply_num==0) autoreply_num=0x80;
CIM_frame.cim_feature=ARDUINO_CIM_FEATURE_ADCREPORT;
CIM_frame.serial_number=autoreply_num;
CIM_frame.reply_code=CMD_EVENT_REPLY;
generate_ADCFrame();
reply_DataFrame();
//DDRB |= (1<<5); PORTB ^= (1<<5); // indicate frame send with led
}
if (check_PINChange_now) // this is updated in the timer ISR !!
{
check_PINChange_now=0;
// has a selected pin changed ?
if ( ((old_PIND ^ PIND) & PIND_Mask) || ((old_PINB ^ PINB) & PINB_Mask))
{
old_PIND=PIND;
old_PINB=PINB;
autoreply_num++;
if (autoreply_num==0) autoreply_num=0x80;
CIM_frame.cim_feature=ARDUINO_CIM_FEATURE_GET_PINVALUES;
CIM_frame.serial_number=autoreply_num;
CIM_frame.reply_code=CMD_EVENT_REPLY;
generate_PINFrame();
reply_DataFrame();
}
}
}
}
int main(void)
{
//Inicializa sistema
setupHardware();
//Inicializa dispositivos
setupDevices();
while(1)
{
//Executa rotinas da funcionalidade
run_system();
//Executa rotina que trata a HIM
run_him();
}
return 0;
}
int main(void)
{
//Incialización del Hardware
setupHardware();
//Trace del RTOS
//traceSTART();
xTaskCreate(taskLed,
(const signed char * const)"taskLed",
configMINIMAL_STACK_SIZE,
NULL,
tskIDLE_PRIORITY+1,
NULL);
//Inicio el Scheduler
vTaskStartScheduler();
while(1);
}
/**
* Program entry point
*/
int main(void) {
setupHardware();
xTaskCreate( vFATFSTimerTask , (signed portCHAR *) "vFATFSTimerTask" , 100, NULL, 2, NULL );
xTaskCreate( taskAdc , (signed portCHAR *) "Adc" , 1024, NULL, 2, NULL );
xTaskCreate( taskWebServer , (signed portCHAR *) "WebServer" , 2048, NULL, 2, NULL );
/* Start the scheduler. */
vTaskStartScheduler();
/* Will only get here if there was insufficient memory to create the idle task. */
while(1){
}
return 1;
}
int main(void)
{
setupHardware();
printf("\r\n++++++++++++++++++++++\r\n");
printf("Megalan Controller 2.3\r\n");
printf("++++++++++++++++++++++\r\n");
initializeVariables();
//wait pir to stabilize
delayMs(1000);
while (1)
{
printf("\r\nAwake\r\n");
if (processIRCommand)
{
handleIR();
}
if (checkPIR() && !pirActive)
{
handleMovement();
}
else if (!checkPIR() && pirActive)
{
handleMovementMissing();
}
if (rtcAlarm)
{
handleRTC();
}
gotoSLEEP();
//wait a bit
//otherwise we may put the device
//to sleep while it is receiving commands!
delayMs(500);
}
}
/** Main program entry point. This routine contains the overall program flow, including initial
* setup of all components and the main program loop.
*/
int main(void)
{
setupHardware();
init_CIM_frame();
LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
sei();
for (;;)
{
parse_CIM_protocol();
HID_Device_USBTask(&Keyboard_HID_Interface);
parse_CIM_protocol();
HID_Device_USBTask(&Mouse_HID_Interface);
parse_CIM_protocol();
HID_Device_USBTask(&Joystick_HID_Interface);
parse_CIM_protocol();
USB_USBTask();
}
}
int main(void) {
// TODO: insert code here
//Incialización del Hardware
setupHardware();
//Trace del RTOS
//traceSTART();
//Creación de las tareas
xTaskCreate( taskLed,
(const signed char * const)"taskLed",
configMINIMAL_STACK_SIZE,
&led,
tskIDLE_PRIORITY+2,
NULL );
xTaskCreate( taskLed,
(const signed char * const)"taskLed2",
configMINIMAL_STACK_SIZE,
&led2,
tskIDLE_PRIORITY+2,
NULL );
xTaskCreate( taskPrintf,
(const signed char * const)"taskPrintf",
configMINIMAL_STACK_SIZE,
NULL,
tskIDLE_PRIORITY+1,
NULL );
//Inicio el Scheduler
vTaskStartScheduler();
while(1);
}
int
main( void )
{
//initialize all
setupHardware();
xSemaphore = xSemaphoreCreateMutex();
//xTaskCreate( ModbusTask, ( signed portCHAR * ) "ModbusTask", USERTASK_STACK_SIZE, NULL, 3, NULL );
//xTaskCreate( SensorsTask, ( signed portCHAR * ) "SensorsTask", USERTASK_STACK_SIZE, NULL, 3, NULL );
xTaskCreate( MainTask, ( signed portCHAR * ) "MainTask", USERTASK_STACK_SIZE, NULL, 4, NULL );
/*
* Start the scheduler.
*/
vTaskStartScheduler();
return 1;
}
int main()
{
// Initialize system.
init_syscalls();
// Set up the LED ports.
vtInitLED();
// Set up hardware.
setupHardware();
// Buffer for events messages.
char eventsMsg[QUEUE_BUF_LEN_LCD];
// LCD task.
startTaskLCD(&dataLCD, LCD_TASK_PRIORITY);
startTimerLCD(&dataLCD);
// Debug - initialize base.
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, "Initializing system", portMAX_DELAY);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, "Initializing LEDs", portMAX_DELAY);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, "Initializing hardware", portMAX_DELAY);
sprintf(eventsMsg, "Starting %s", taskNameLCD);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
sprintf(eventsMsg, "Starting %s", timerNameLCD);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
// I2C task.
sprintf(eventsMsg, "Starting %s", taskNameI2C);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
if(vtI2CInit(&vtI2C0, 0, MONITOR_TASK_PRIORITY, I2C_BITRATE) != vtI2CInitSuccess)
{
sprintf(eventsMsg, "Unable to create queue for %s", taskNameI2C);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
VT_HANDLE_FATAL_ERROR(0);
}
// Command task.
sprintf(eventsMsg, "Starting %s", taskNameCommand);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTaskCommand(&dataCommand, COMMAND_TASK_PRIORITY, &vtI2C0, &dataLCD);
sprintf(eventsMsg, "Starting %s", timerNameCommand);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTimerCommand(&dataCommand);
// Locate task.
sprintf(eventsMsg, "Starting %s", taskNameLocate);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTaskLocate(&dataLocate, SENSOR_TASK_PRIORITY, &dataLCD, &dataCommand);
sprintf(eventsMsg, "Starting %s", timerNameLocate);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTimerLocate(&dataLocate);
// Sensors task.
sprintf(eventsMsg, "Starting %s", taskNameSensors);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTaskSensors(&dataSensors, SENSOR_TASK_PRIORITY, &vtI2C0, &dataLCD, &dataLocate);
sprintf(eventsMsg, "Starting %s", timerNameSensors);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTimerSensors(&dataSensors);
// Conductor task.
sprintf(eventsMsg, "Starting %s", taskNameConductor);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
startTaskConductor(&dataConductor, CONDUCTOR_TASK_PRIORITY, &vtI2C0, &dataLCD, &dataSensors, &dataLocate);
// Schedlar task.
sprintf(eventsMsg, "Starting %s", taskNameScheduler);
sendValueMsgLCD(&dataLCD, MSG_TYPE_LCD_EVENTS, QUEUE_BUF_LEN_LCD, eventsMsg, portMAX_DELAY);
vTaskStartScheduler();
// Shouldn't reach this part unless insufficient memory.
for(;;);
}
/**
* Program entry point
*/
int main(void) {
setupHardware();
/*
* Start the tasks defined within this file/specific to this demo.
*/
xTaskCreate( vUserTask1, ( signed portCHAR * ) "Task1", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( vUserTask2, ( signed portCHAR * ) "Task2", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( working_task, ( signed portCHAR * ) "Task3", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( vUserTask4, ( signed portCHAR * ) "Task4", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( spi_task, ( signed portCHAR * ) "Task5", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( protocol_task_runner, ( signed portCHAR * ) "Task6", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( motor_task, ( signed portCHAR * ) "Task7", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( uart0_receive_task_runner, ( signed portCHAR * ) "Task8", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( regulation_task_runner, ( signed portCHAR * ) "Task8", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate( joystick_task_runner, ( signed portCHAR * ) "Joystick", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
/*
* Setup semaphores.
*/
lcd_buffer_mutex = xSemaphoreCreateMutex();
if ( lcd_buffer_mutex == NULL )
{
led_red_on();
while(1);
}
lcd_keyboard_port_mutex = xSemaphoreCreateMutex();
if ( lcd_keyboard_port_mutex == NULL )
{
led_red_on();
while(1);
}
motor_one_speed_mutex = xSemaphoreCreateMutex();
if ( motor_one_speed_mutex == NULL )
{
led_red_on();
while(1);
}
motor_two_speed_mutex = xSemaphoreCreateMutex();
if ( motor_two_speed_mutex == NULL )
{
led_red_on();
while(1);
}
motor_one_direction_mutex = xSemaphoreCreateMutex();
if ( motor_one_direction_mutex == NULL )
{
led_red_on();
while(1);
}
motor_two_direction_mutex = xSemaphoreCreateMutex();
if ( motor_two_direction_mutex == NULL )
{
led_red_on();
while(1);
}
motor_one_position_mutex = xSemaphoreCreateMutex();
if ( motor_one_position_mutex == NULL )
{
led_red_on();
while(1);
}
motor_two_position_mutex = xSemaphoreCreateMutex();
if ( motor_two_position_mutex == NULL )
{
led_red_on();
while(1);
}
y_pos_mutex = xSemaphoreCreateMutex();
if ( y_pos_mutex == NULL )
{
led_red_on();
while(1);
}
x_pos_mutex = xSemaphoreCreateMutex();
if ( x_pos_mutex == NULL )
{
led_red_on();
while(1);
}
y_target_pos_mutex = xSemaphoreCreateMutex();
if ( y_target_pos_mutex == NULL )
{
led_red_on();
while(1);
}
x_target_pos_mutex = xSemaphoreCreateMutex();
if ( x_target_pos_mutex == NULL )
{
led_red_on();
while(1);
}
/*
* Setup queues.
*/
motor_command_queue = xQueueCreate(16, sizeof( motor_command ) );
if (motor_command_queue == NULL)
{
led_red_on();
while(1);
}
motor_event_queue = xQueueCreate(16, sizeof( motor_event ) );
if (motor_event_queue == NULL)
{
led_red_on();
while(1);
}
spi_input_queue = xQueueCreate(16, sizeof( INT16U ) );
if (spi_input_queue == NULL)
{
led_red_on();
while(1);
}
spi_output_queue = xQueueCreate(16, sizeof( INT16U ) );
if (spi_output_queue == NULL)
{
led_red_on();
while(1);
}
ui_event_queue = xQueueCreate(16, sizeof( ui_event ) );
if (ui_event_queue == NULL)
{
led_red_on();
while(1);
}
uart_command_queue = xQueueCreate(16, sizeof( uart_command ) );
if (uart_command_queue == NULL)
{
led_red_on();
while(1);
}
/*
* Start the scheduler.
*/
vTaskStartScheduler();
/*
* Will only get here if there was insufficient memory to create the idle task.
*/
return 1;
}
__attribute__((weak)) void setupXpress()
{
fprintf(stderr,"Using default core0 Xpress FPGA Hardware\n");
setupHardware("core0");
}
uint8_t process_ARE_frame(uint8_t status_code)
{
uint8_t ack_needed;
uint8_t data_size=0;
uint8_t command;
// LEDs_ToggleLEDs(LED0); // indicate correct frame
command=(uint8_t)ARE_frame.request_code;
CIM_frame.cim_feature=ARE_frame.cim_feature;
CIM_frame.serial_number=ARE_frame.serial_number;
CIM_frame.reply_code=(((uint16_t)status_code)<<8) + command;
data_size=(uint8_t)ARE_frame.data_size;
ack_needed=1;
// if ((status_code & (CIM_ERROR_INVALID_ARE_VERSION | CIM_ERROR_CRC_MISMATCH)) == 0)
if ((status_code & CIM_ERROR_INVALID_ARE_VERSION) == 0)
{
// UART_Print(" feature "); UART_Putchar(command);
// no serious packet error
switch (command) { // process requested command
case CMD_REQUEST_FEATURELIST:
if (data_size==0) {
reply_FeatureList(); // reply requested feature list
ack_needed=0;
} else status_code |= CIM_ERROR_INVALID_FEATURE;
break;
case CMD_REQUEST_RESET_CIM:
if (data_size!=0) status_code |= CIM_ERROR_INVALID_FEATURE;
break;
case CMD_REQUEST_START_CIM:
if (data_size==0) {
cli();
init_CIM_frame();
setupHardware();
start_timer1();
sei();
} else status_code |= CIM_ERROR_INVALID_FEATURE;
break;
case CMD_REQUEST_STOP_CIM:
if (data_size==0) {
first_packet=1; // reset first frame indicator etc.
stop_timer1();
} else status_code |= CIM_ERROR_INVALID_FEATURE;
break;
case CMD_REQUEST_READ_FEATURE: // read feature from CIM
switch (ARE_frame.cim_feature) {
case TEENSY_CIM_FEATURE_UNIQUENUMBER: // read unique serial number
if (data_size==0) {
reply_UniqueNumber();
ack_needed=0;
} else status_code |= CIM_ERROR_INVALID_FEATURE;
break;
default: // not a valid read feature;
status_code |= CIM_ERROR_INVALID_FEATURE;
}
break;
case CMD_REQUEST_WRITE_FEATURE: // write feature to CIM
switch (ARE_frame.cim_feature) { // which feature address ?
case TEENSY_CIM_FEATURE_MODE_SELECTION:
if (data_size==2) {
cli();
selection = (uint16_t)ARE_frame.data[0];
selection += ((uint16_t)ARE_frame.data[1])<<8;
sei();
}
break;
case TEENSY_CIM_FEATURE_SET_THRESHOLD:
if (data_size==2) {
cli();
threshold = (uint16_t)ARE_frame.data[0];
threshold += ((uint16_t)ARE_frame.data[1])<<8;
sei();
}
break;
/*case TEENSY_CIM_FEATURE_SET_ADCPERIOD:
if (data_size==2) {
cli();
ADC_updatetime= (uint16_t)ARE_frame.data[0];
ADC_updatetime+= ((uint16_t)ARE_frame.data[1])<<8;
sei();
}
break;*/
default: // not a valid write feature;
status_code |= CIM_ERROR_INVALID_FEATURE;
}
}
}
if (status_code & CIM_ERROR_INVALID_FEATURE) { // invalid data size or feature
// LEDs_ToggleLEDs(LED5); // indicate wrong feature
// UART_Print(" invalid data size or no feature ");
}
if (ack_needed) {
reply_Acknowledge();
}
return(1);
}
/**
* Program entry point
*/
int main(void) {
setupHardware();
delay_ms(1000);
vSemaphoreCreateBinary(xSemaphoreGPS);
xSemaphoreTake(xSemaphoreGPS,0);
#ifdef USE_MODULE_GPS
xTaskCreate( vGpsTask, ( signed portCHAR * ) "gpsTask", USERTASK_STACK_SIZE, NULL, 3, &xGpsTaskHandle );
#endif
// To tak naprawde jest task od DACa
xTaskCreate( vLedTask, ( signed portCHAR * ) "Led", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
// /*
// * Start the tasks defined
// */
//
//
// xTaskCreate( vMeasTask, ( signed portCHAR * ) "measTask", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY+2, NULL );
//
//
// /*
// * Start the scheduler.
// */
// vTaskStartScheduler();
//
// /*
// * Will only get here if there was insufficient memory to create the idle task.
// */
// uint8_t i = 10;
// uint8_t returnVal;
// uint32_t bytes;
// while( GSM_requestTimeSynchro() );
//
// while(1)
// {
// getRtcFullTime();
//
// for(i=10; i!=0; i--)
// {
// getSHT21Hum();
// getSHT21Temp();
// delay_ms(30000);
// }
//
// returnVal = 1;
//
// for(i=0;((i<10)&(returnVal!=0)); i++)
// {
// returnVal = GSM_GPRSConnection();
// }
// bytes = debug();
// }
/*
* Start the tasks defined within this file/specific to this demo.
*/
// xTaskCreate( vMeasTask, ( signed portCHAR * ) "measure", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
// xTaskCreate( vUserTask2, ( signed portCHAR * ) "Task2", USERTASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
/*
* Start the scheduler.
*/
vTaskStartScheduler();
/*
* Will only get here if there was insufficient memory to create the idle task.
*/
while(1)
{
}
return 1;
}
int main(void)
{
uint8_t loops_ocr1a = 0;
/* Cache for OCR1A when PWM is disabled */
uint16_t OCR1A_save;
bool usb_initialized = 0;
/* Is HV supply voltage reached? If not take bigger steps */
bool hv_reached = 0;
setupHardware();
sei(); /* Enable interrupts */
while(1)
{
if(!usb_initialized) {
/* Check if USB power is connected */
if (bit_is_set(PINB,4)) {
/* USB initialization */
USB_Init();
CDC_Device_CreateStream(&VirtualSerial_CDC_Interface, &USBSerialStream);
usb_initialized = 1;
}
}
if (usb_connected) {
/* Check mail */
usb_rx();
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
/* Send counter over usb and reset */
if (send_count && tx_flag) {
tx_flag = 0;
/* Print number of events and time of last event */
fprintf(&USBSerialStream, "c%3d,%3d\r\n", tx_count, tx_event_time);
/* TODO: LCD */
}
}
/* HV supply feedback */
if( use_comparator ) {
/* Check if output voltage is over threshold */
if (bit_is_set(ACSR, ACO)) {
/* Not over 400V */
if ((loops_ocr1a % 10) == 0) {
if (OCR1A < 100) {
if (!hv_reached) {
OCR1A += 15;
} else {
OCR1A += 1;
}
}
if (OCR1A < 500) {
if (!hv_reached) {
OCR1A += 10;
} else {
OCR1A += 1;
}
}
}
} else if((loops_ocr1a == 100)) {
/* Required voltage reached, decrease step size */
hv_reached = 1;
/* Decrease OCR1A when after some time to avoid creeping upwards */
if (OCR1A > 2) {
OCR1A--;
}
/* Reset loop counter */
loops_ocr1a = 0;
}
loops_ocr1a++;
}
if (buzzer_enabled && tick_pending) {
bit_set(PORTC, 7);
tick_pending = 0;
}
sleep1ms();
bit_clear(PORTC, 7);
}
}
// Main
int main(void)
{
setupHardware();
LEDs_TurnOnLEDs(LEDMASK_USB_NOTREADY);
sei();
while(1)
{
// Perform the HID specific tasks
hidTask();
// Perform the USB library tasks
USB_USBTask();
// Show command processing activity on led 1 (Rx)
if (activityIndicatorRx > 0)
{
// See if this is the first loop
if (activityIndicatorRx == 1) LEDs_TurnOnLEDs(LEDS_LED1);
activityIndicatorRx++;
if (activityIndicatorRx > 5000) // Flash length is in loops (16 bit maximum)
{
// Turn the indicator off
activityIndicatorRx = 0;
LEDs_TurnOffLEDs(LEDS_LED1);
}
}
// Show command processing activity on led 2 (Tx)
if (activityIndicatorTx > 0)
{
// See if this is the first loop
if (activityIndicatorTx == 1) LEDs_TurnOnLEDs(LEDS_LED2);
activityIndicatorTx++;
if (activityIndicatorTx > 5000) // Flash length is in loops (16 bit maximum)
{
// Turn the indicator off
activityIndicatorTx = 0;
LEDs_TurnOffLEDs(LEDS_LED2);
}
}
// Show flashing for system/blink command on led 3 (L)
if (activityIndicatorL > 0)
{
// See if this is the first loop
if (activityIndicatorL == 1)
{
LEDs_TurnOnLEDs(LEDS_LED3);
activityIndicatorLblink = 0;
}
// Increment the activity counters
activityIndicatorL++;
activityIndicatorLblink++;
// Control the flashing of the LED
if (activityIndicatorLblink == 5000) LEDs_TurnOffLEDs(LEDS_LED3);
if (activityIndicatorLblink == 10000)
{
LEDs_TurnOnLEDs(LEDS_LED3);
activityIndicatorLblink = 0;
}
if (activityIndicatorL > 200000) // Flash length is in loops (32 bit maximum)
{
// Turn the indicator off
activityIndicatorL = 0;
// Default state of the L LED is on
LEDs_TurnOnLEDs(LEDS_LED3);
}
}
}
}
void
RoboyListenToSound::updateLoudness() {
Loudness = 0;
/// inserted from C code
int size;
int direction;
unsigned int val;
snd_pcm_t *pHandle;
snd_pcm_hw_params_t *pParams;
snd_pcm_uframes_t frames_per_period = 32;
char *pBuffer;
// allocate buffers for last (FILTER_SIZE) values for first and second channel:
double* pBuffer_lastvalues_first = (double *) malloc(sizeof(double)*FILTER_SIZE);
double* pBuffer_lastvalues_second = (double *) malloc(sizeof(double)*FILTER_SIZE);
// initialize buffer
for (int i = 0; i < FILTER_SIZE; i++)
{
pBuffer_lastvalues_first[i] = 0;
pBuffer_lastvalues_second[i] = 0;
}
int current_pointer_first = 0;
int current_pointer_second = 0;
short threshold_for_sounddetection = 16000;
short amplitude_tolerance_to_count_as_same_sound = 1000;
bool threshold_reached = false;
bool sound_detected = false;
// TODO: a function to read out sound_detected and sound_angle has to be written,
// which can be called by other routines. When this function gets called and
// sound_detected is true, sound_detected can be set back to false afterwards
// and sound_angle can be set back to 0 again.
double sound_angle = 0; // as a first approach: right is 1, left is -1
double fake_loudness_first = 0;
double fake_loudness_second = 0;
double SUM_last_samples_squared_first = 0;
double SUM_last_samples_squared_second = 0;
double rms_last_samples_first = 0;
double rms_last_samples_second = 0;
double sample_first = 0;
double sample_second = 0;
double channel_difference = 0;
int i;
int errorCode = setupHardware(&pHandle, &pParams, NUM_CHANNELS, "default", &direction, &frames_per_period, &val, DEBUG_OUTPUT);
if (errorCode)
{
printf("setting up hardware failed (%i)\n", errorCode);
return;
}
else
{
printf("# set up hardware successful\n");
}
/* Use a buffer which holds one period (not the same as the internal ALSA buffer!) */
snd_pcm_hw_params_get_period_size(pParams, &frames_per_period, &direction);
size = frames_per_period * 2 * NUM_CHANNELS; /* 2 bytes/sample, X channels */
pBuffer = (char *) malloc(size);
if (pBuffer == NULL)
{
printf("Memory for buffer could not be allocated!\n");
return;
}
snd_pcm_hw_params_get_period_time(pParams, &val, &direction);
std::cout << "period length: " << val << "\n";
////
while (1) {
try {
//// inserted from C code
// read interleaved frames
errorCode = snd_pcm_readi(pHandle, pBuffer, frames_per_period);
if (errorCode == -EPIPE)
{
// EPIPE means overrun
fprintf(stderr, "overrun occurred\n");
snd_pcm_prepare(pHandle);
}
else if (errorCode < 0)
{
fprintf(stderr, "error from read: %s\n", snd_strerror(errorCode));
}
else if (errorCode != (int)frames_per_period)
{
fprintf(stderr, "short read, read %d frames_per_period\n", errorCode);
}
// loop over all samples (each frame consists of two two-byte samples, one for left, one for right)
for (i = 0; i < (int)frames_per_period * (int)NUM_CHANNELS; i++)
{
//first channel (always first channel if only 1 channel):
if (i%2 == 0 || NUM_CHANNELS == 1)
{
// to build the sum of the current and all the FILTER_SIZE-1 last squared samples,
// first we subtract the last one in the queue
// (which is exactly at position current_pointer):
SUM_last_samples_squared_first = SUM_last_samples_squared_first - pBuffer_lastvalues_first[current_pointer_first]
*pBuffer_lastvalues_first[current_pointer_first];
// then we can overwrite the current position:
sample_first = (double) *( (short*) &(pBuffer[i*2]) );
pBuffer_lastvalues_first[current_pointer_first] = sample_first;
fake_loudness_first += abs(sample_first);
// then we add the squared current sample:
SUM_last_samples_squared_first = SUM_last_samples_squared_first + sample_first*sample_first;
// for the rms we have to take the average of the 100 squared values and then take the root:
// (thom: i'd say this should exactly be the standard deviation)
rms_last_samples_first = sqrt(SUM_last_samples_squared_first/FILTER_SIZE);
//std::cout << rms_last_samples_first;
//std::cout << sample_first;
//std::cout << "\n";
if (current_pointer_first == FILTER_SIZE-1)
{
current_pointer_first = 0;
}
else
{
current_pointer_first++;
}
}
// repeat everything for second channel:
else
{
// to build the sum of the current and all the FILTER_SIZE-1 last squared samples,
// first we subtract the last one in the queue
// (which is exactly at position current_pointer):
SUM_last_samples_squared_second = SUM_last_samples_squared_second - pBuffer_lastvalues_second[current_pointer_second]
*pBuffer_lastvalues_second[current_pointer_second];
// then we can overwrite the current position:
sample_second = (double) *( (short*) &(pBuffer[i*2]) );
pBuffer_lastvalues_second[current_pointer_second] = sample_second;
fake_loudness_second += abs(sample_second);
// then we add the squared current sample:
SUM_last_samples_squared_second = SUM_last_samples_squared_second + sample_second*sample_second;
// for the rms we have to take the average of the 100 squared values and then take the root:
rms_last_samples_second = sqrt(SUM_last_samples_squared_second/FILTER_SIZE);
//std::cout << rms_last_samples_second;
//std::cout << sample_second;
//std::cout << "\n";
if (current_pointer_second == FILTER_SIZE-1)
{
current_pointer_second = 0;
}
else
{
current_pointer_second++;
}
}
//// check if loudness treshold was reached ////
// in case treshold was not reached in the last values,
// check if a loud sound got detected in the right or left channel
if (threshold_reached == false)
{
if (rms_last_samples_first > threshold_for_sounddetection || rms_last_samples_second > threshold_for_sounddetection)
{
threshold_reached = true;
if (rms_last_samples_first > rms_last_samples_second)
{
sound_angle = 1;
}
else
{
sound_angle = -1;
}
sound_detected = true;
}
// in case treshold was already reached in the last values,
// set it back to "not reached" in case both values get down
// below (treshold minus tolerance)
}
else
{
if (rms_last_samples_first < (threshold_for_sounddetection - amplitude_tolerance_to_count_as_same_sound)
&& rms_last_samples_second < (threshold_for_sounddetection - amplitude_tolerance_to_count_as_same_sound))
{
threshold_reached = false;
}
}
// calculate the difference between the samples of the two channels
// (just for debugging)
channel_difference = sample_first - sample_second;
//if (loops == 1) {std::cout << channel_difference << "\n";}
//if (loops < 10) {std::cout << sample_first << "\n";}
//if (loops == 1) {std::cout << sample_second << "\n";}
} // end for-loop
//std::cout << "threshold_reached = " << threshold_reached;
//std::cout << "\n";
//std::cout << "sound_detected = " << sound_detected;
//std::cout << "\n";
//std::cout << "sound_angle = " << sound_angle;
//std::cout << "\n";
fake_loudness_first = fake_loudness_first/((double)frames_per_period * 2);
fake_loudness_second = fake_loudness_second/((double)frames_per_period * 2);
//std::cout << fake_loudness;
//std::cout << "\n";
////
Loudness = rms_last_samples_first;
//std::cout << "Loudness : " << Loudness << std::endl;
SoundIsLoud = threshold_reached;
//std::cout << "SoundIsLoud : " << SoundIsLoud << std::endl;
boost::this_thread::interruption_point();
} catch(boost::thread_interrupted const&) {
std::cout << "update Loudness INTERRUPTED" << std::endl;
break;
}
} // end while
}
