float HCSR04::ping()
{
taskDISABLE_INTERRUPTS();
/* Trigger low */
digitalWrite(TRIG_PIN, LOW);
/* Delay 2 us */
delayMicroseconds(2);
/* Trigger high for 10us */
digitalWrite(TRIG_PIN, HIGH);
/* Delay 10 us */
delayMicroseconds(10);
/* Trigger low */
digitalWrite(TRIG_PIN, LOW);
taskENABLE_INTERRUPTS();
float distance = NO_ECHO;
// TODO: Depending on the period, this might cause extra delays!
if (xSemaphoreTake(sensorReadSignal, MAX_ULTRASOUND_DURATION_MS))
{
// Get distance in us and convert us to cm
distance = (float)ultrasound_duration * HCSR04_NUMBER;
}
// every value > range is out of range!
if (distance > HCSR04_MAX_RANGE)
return NO_ECHO;
if (distance < HCSR04_MIN_RANGE)
return HCSR04_MIN_RANGE;
/* Return distance */
return distance;
}
void restore_global_interrupts() {
#ifdef USE_FREERTOS
taskENABLE_INTERRUPTS();
#else
ARM_AR_INT_BITS_SET(old_value);
#endif
}
bool LineRecv(unsigned char* pRecvBuf,unsigned long ulLength)
{
//ASSERT(ulLength>=ITEM_LENGTH);
//ASSERT(pRecvBuf != NULL);
taskDISABLE_INTERRUPTS();
unsigned char* pLine;
portBASE_TYPE ptRet = xQueueReceive(s_QueueLine,&pLine,0);
if (ptRet != pdPASS) {
taskENABLE_INTERRUPTS();
return false;
}
memcpy(pRecvBuf, pLine, ITEM_LENGTH);
ptRet = xQueueSendToBack(s_QueueBuff,&pLine,0);
ASSERT(ptRet == pdPASS);
taskENABLE_INTERRUPTS();
return true;
}
void vAssertCalled( unsigned long ulLine, const char * const pcFileName )
{
/* Parameters are not used. */
( void ) ulLine;
( void ) pcFileName;
taskDISABLE_INTERRUPTS();
#if defined (__WIN32__)
__asm volatile( "int $3" );
#endif
taskENABLE_INTERRUPTS();
}
void vAssertCalled( void )
{
const unsigned long ulLongSleep = 1000UL;
volatile uint32_t ulBlockVariable = 0UL;
/* Setting ulBlockVariable to a non-zero value in the debugger will allow
this function to be exited. */
taskDISABLE_INTERRUPTS();
{
while( ulBlockVariable == 0UL )
{
Sleep( ulLongSleep );
}
}
taskENABLE_INTERRUPTS();
}
/*---------------------------------------------------------------------------*
* Routine: PlayAudio
*---------------------------------------------------------------------------*
* Description:
* Play a tone for the given length, staying here until done.
* Inptus:
* TUInt32 aHz -- Tone in Hz
* TUInt32 aMS -- Duration of tone
*---------------------------------------------------------------------------*/
void PlayAudio(TUInt32 aHz, TUInt32 aMS)
{
#if OPTION_USE_GPIO_LINES_FOR_AUDIO
static HAL_GPIOPort **p_gpio0;
TUInt32 n;
TUInt32 start = UEZTickCounterGet();
TUInt32 count;
HALInterfaceFind("GPIO0", (T_halWorkspace **)&p_gpio0);
(*p_gpio0)->SetOutputMode(p_gpio0, (1<<26));
// Wait for the parameters to be ready
n = (G_subTickCount*1000/2)/aHz;
taskDISABLE_INTERRUPTS();
G_dummyCounter = 0;
// Wait for first tick count
while ((T1IR&2)==0)
{}
while (aMS--) {
T1IR = 2;
while (!(T1IR & 2)) {
count = n;
while (count--)
{ G_dummyCounter++; }
(*p_gpio0)->Clear(p_gpio0, (1<<26));
count = n;
while (count--)
{ G_dummyCounter++; }
(*p_gpio0)->Set(p_gpio0, (1<<26));
}
}
taskENABLE_INTERRUPTS();
#else
if (!G_tg)
CalibrateAudioTiming();
//PWMAudio(aHz, (aMS+10)/10, 0);
if (G_tg)
UEZToneGeneratorPlayTone(
G_tg,
TONE_GENERATOR_HZ(aHz),
aMS);
else
UEZTaskDelay(aMS);
#endif
}
void TerminalTask( void *pvParameters )
{
terminalQueue = xQueueCreate(TERMINAL_QUEUE_LEN, MAX_CHARS);
// Initialize memory (in stack) for message.
static unsigned char string[MAX_CHARS];
// Try to receive message, put the task to sleep for at most RXTIMEOUT ticks if queue is empty.
for(;;)
{
if (xQueueReceive(terminalQueue, string, TERMINAL_QUEUE_TIMEOUT))
{
taskDISABLE_INTERRUPTS();
USART_puts(USART1, (volatile char*)string);
taskENABLE_INTERRUPTS();
}
}
}
static void new_multimeter_sample(const measure_data_t* data)
{
int channel = ch_generic2internal(data->ch);
static portTickType last_update[NUMBER_OF_CHANNELS] = { 0 };
if((last_update[channel] + CONFIG_DISPLAY_MULTIMETER_REFRESH_TIME) < xTaskGetTickCount())
{
static char buffer[15];
static const int max_mV = 3300;
int mV = (data->data[0]*max_mV)/ADC_MAX;
int Line;
char ch;
last_update[channel] = xTaskGetTickCount();
switch (channel)
{
case input_channel0:
Line = Line3;
ch = 'A';
break;
case input_channel1:
Line = Line4;
ch = 'B';
break;
default:
ipc_watchdog_signal_error(0);
return;
}
sprintf (buffer, "Channel %c: %i.%03i V", ch, mV/1000,mV%1000);
xSemaphoreTake(lcdLock, portMAX_DELAY);
taskDISABLE_INTERRUPTS();
TIM_ITConfig(TIM2, TIM_IT_Update, DISABLE);
GLCD_setTextColor(Black);
GLCD_displayStringLn(Line, (unsigned char *) buffer);
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
taskENABLE_INTERRUPTS();
xSemaphoreGive(lcdLock);
}
}
void prvTaskExtCtrl(void* pvParameters)
{
while(1)
{
/* Store value converted from mdps to dps */
xSemaphoreTake(xExtCtrlSem, 0);
taskDISABLE_INTERRUPTS();
xExtCtrl.pitch = (xPitchTick - EXTCTRL_TIM_COUNT_MIN) / (EXTCTRL_TIM_COUNT_MAX - EXTCTRL_TIM_COUNT_MIN) * 100;
xExtCtrl.roll = (xRollTick - EXTCTRL_TIM_COUNT_MIN) / (EXTCTRL_TIM_COUNT_MAX - EXTCTRL_TIM_COUNT_MIN) * 100;
xExtCtrl.yaw = (xYawTick - EXTCTRL_TIM_COUNT_MIN) / (EXTCTRL_TIM_COUNT_MAX - EXTCTRL_TIM_COUNT_MIN) * 100;
taskENABLE_INTERRUPTS();
/* Critical section */
xSemaphoreGive(xExtCtrlSem);
/* Delay specified period of time */
vTaskDelay(1000 / TASK_EXTCTRL_SAMPLING_RATE / portTICK_PERIOD_MS);
}
}
void vAssertCalled( const char * pcFile, unsigned long ulLine )
{
volatile unsigned long ul = 0;
( void ) pcFile;
( void ) ulLine;
taskDISABLE_INTERRUPTS();
{
/* Set ul to a non-zero value using the debugger to step out of this
function. */
while( ul == 0 )
{
portNOP();
}
}
taskENABLE_INTERRUPTS();
}
//*=========================================================
//* READ
//*=========================================================
//*----------------------------------------------------------------------------
//* \fn AT91F_TWI_Read
//* \brief Read n bytes from a slave device
//*----------------------------------------------------------------------------
int AT91F_TWI_Read(unsigned int devaddress,unsigned int address,unsigned char *data,int size)
{
unsigned int SizeReaded;
volatile unsigned int status;
if( TWI_Semaphore != NULL )
{
if( xSemaphoreTake( TWI_Semaphore, ( portTickType ) 2000 ) == pdTRUE )
{
SizeReaded = 0;
AT91PS_TWI TWI = AT91C_BASE_TWI;
TWI_DataPointer = data;
TWI_Bytes2Transfere = size;
TWI_DeviceAddr = devaddress;
if(TWI_Bytes2Transfere>1)TWI_ConitiosTransaction = 1;
taskDISABLE_INTERRUPTS();
TWI->TWI_IADR = address;
TWI->TWI_MMR = TWI_DeviceAddr | AT91C_TWI_IADRSZ_2_BYTE | AT91C_TWI_MREAD;
TWI_TransferStatus = ON_READ_DATA;
AT91F_TWI_EnableIt(TWI, AT91C_TWI_TXCOMP | AT91C_TWI_NACK | AT91C_TWI_OVRE | AT91C_TWI_UNRE);
TWI->TWI_CR = AT91C_TWI_START | AT91C_TWI_MSEN | AT91C_TWI_STOP ;
status = TWI->TWI_SR;
taskENABLE_INTERRUPTS();
xQueueReceive( TWI_QUEUE, &( SizeReaded ), ( portTickType ) AT91C_TWI_TMO_VOL(size) );
if( TWI_TransferStatus != FREE)
{
// SYSTEM_ERROR(TWI_TMO_Error);
RESET_TWI();
} ;
TWI_TransferStatus = FREE;
xSemaphoreGive( TWI_Semaphore );
}
else
{
//SYSTEM_ERROR(TWI_SEM_Error);
}
}
return SizeReaded;
}
//*=========================================================
//* WRITE
//*=========================================================
//*----------------------------------------------------------------------------
//* \fn AT91F_TWI_Write
//* \brief Send n bytes to a slave device
//*----------------------------------------------------------------------------
int AT91F_TWI_Write(unsigned int devaddress,unsigned int address,unsigned char *data2send, int size)
{
unsigned int SizeNotWrited = 0;
//---------------------------------------------------------
if( TWI_Semaphore != NULL )
{
if( xSemaphoreTake( TWI_Semaphore, ( portTickType ) 2000 ) == pdTRUE )
{
TWI_TransferStatus = ON_WRITE_DATA;
SizeNotWrited = 0;
TWI_DataPointer = data2send;
TWI_Bytes2Transfere = size;
TWI_DeviceAddr = devaddress;
TWI_TransferStatus = ON_WRITE_DATA;
taskDISABLE_INTERRUPTS();
AT91C_BASE_TWI->TWI_MMR = ( TWI_DeviceAddr | AT91C_TWI_IADRSZ_2_BYTE ) & ~AT91C_TWI_MREAD;
AT91C_BASE_TWI->TWI_IADR = address;
AT91C_BASE_TWI->TWI_THR = *(TWI_DataPointer++);
AT91F_TWI_EnableIt(AT91C_BASE_TWI,AT91C_TWI_TXRDY | AT91C_TWI_NACK);
AT91C_BASE_TWI->TWI_CR = AT91C_TWI_START ;
taskENABLE_INTERRUPTS();
xQueueReceive( TWI_QUEUE, &( SizeNotWrited ), ( portTickType ) AT91C_TWI_TMO_VOL(size));
if( TWI_TransferStatus != FREE)
{
// SYSTEM_ERROR(TWI_TMO_Error);
RESET_TWI();
};
TWI_TransferStatus = FREE;
xSemaphoreGive( TWI_Semaphore );
}
else
{
//SYSTEM_ERROR(TWI_SEM_Error);
}
}
return SizeNotWrited;
}
static void ReadParameters(void) // read parameters requested by the user in the NMEA sent.
{ if((!NMEA.hasCheck()) || NMEA.isChecked() )
{ const char *Parm; int8_t Val;
Parm = (const char *)NMEA.ParmPtr(0); // [0..15] aircraft-type: 1=glider, 2=towa plane, 3=helicopter, ...
if(Parm)
{ Val=Read_Hex1(Parm[0]);
if( (Val>=0) && (Val<16) ) Parameters.setAcftType(Val); }
Parm = (const char *)NMEA.ParmPtr(1); // [0..3] addr-type: 1=ICAO, 2=FLARM, 3=OGN
if(Parm)
{ Val=Read_Hex1(Parm[0]);
if( (Val>=0) && (Val<4) ) Parameters.setAddrType(Val); }
Parm = (const char *)NMEA.ParmPtr(2); // [HHHHHH] Address (ID): 6 hex digits, 24-bit
uint32_t Addr;
int8_t Len=Read_Hex(Addr, Parm);
if( (Len==6) && (Addr<0x01000000) ) Parameters.setAddress(Addr);
Parm = (const char *)NMEA.ParmPtr(3); // [0..1] RFM69HW (up to +20dBm) or W (up to +13dBm)
if(Parm)
{ Val=Read_Dec1(Parm[0]);
if(Val==0) Parameters.clrTxTypeHW();
else if(Val==1) Parameters.setTxTypeHW(); }
Parm = (const char *)NMEA.ParmPtr(4); // [dBm] Tx power
int32_t TxPower;
Len=Read_SignDec(TxPower, Parm);
if( (Len>0) && (TxPower>=(-10)) && (TxPower<=20) ) Parameters.setTxPower(TxPower);
Parm = (const char *)NMEA.ParmPtr(5); // [Hz] Tx/Rx frequency correction
int32_t FreqCorr;
Len=Read_SignDec(FreqCorr, Parm);
if( (Len>0) && (FreqCorr>=(-100000)) && (FreqCorr<=100000) ) Parameters.RFchipFreqCorr = (FreqCorr<<8)/15625;
taskDISABLE_INTERRUPTS(); // disable all interrupts: Flash can not be read while being erased
IWDG_ReloadCounter(); // kick the watch-dog
Parameters.WriteToFlash(); // erase and write the parameters into the last page of Flash
if(Parameters.ReadFromFlash()<0) Parameters.setDefault(); // read the parameters back: if invalid, set defaults
taskENABLE_INTERRUPTS(); // bring back interrupts and the system
}
PrintParameters();
}
void prvSystemSleep( TickType_t xExpectedIdleTime )
{
uint32_t ulSleepTime;
eSleepModeStatus eSleepStatus;
/* A simple WFI() is executed in any of the cases below:
* 1. the system has just booted and the dg_configINITIAL_SLEEP_DELAY_TIME has not yet
* passed
* 2. the XTAL32K is used as the LP clock, the system has just woke up after clockless
* sleep and the LP clock has not yet settled.
*/
if( !cm_lp_clk_is_avail() ) {
__WFI(); // Wait for an interrupt...
return;
}
if (dg_configUSE_LP_CLK == LP_CLK_RCX) {
// Update if a previous calibration was running and is finished.
if (cm_rcx_calibration_is_on) {
if (cm_calibrate_rcx_update()) {
return;
}
}
}
/*
* Calculate the sleep time
*/
ulSleepTime = pm_conv_ticks_2_prescaled_lpcycles(xExpectedIdleTime);
/* Enter a critical section that will not effect interrupts bringing the MCU
* out of sleep mode.
*/
taskDISABLE_INTERRUPTS();
DBG_CONFIGURE_LOW(CMN_TIMING_DEBUG, CMNDBG_CRITICAL_SECTION);
DBG_SET_HIGH(CPM_USE_TIMING_DEBUG, CPMDBG_SLEEP_ENTER);
/* Ensure it is still ok to enter the sleep mode. */
eSleepStatus = eTaskConfirmSleepModeStatus();
if( eSleepStatus == eAbortSleep ) {
DBG_SET_LOW(CPM_USE_TIMING_DEBUG, CPMDBG_SLEEP_ENTER);
/* A task has been moved out of the Blocked state since this macro was
* executed, or a context switch is being held pending. Do not enter a
* sleep state. Restart the tick and exit the critical section.
*/
taskENABLE_INTERRUPTS();
}
else {
#if (dg_configIMAGE_SETUP == DEVELOPMENT_MODE)
uint32_t primask;
#endif
if( eSleepStatus == eNoTasksWaitingTimeout )
{
/* It is not necessary to configure an interrupt to bring the
* microcontroller out of its low power state at a fixed time in the
* future.
* Enter the low power state.
*/
pm_sleep_enter( 0 );
}
else
{
/* Configure an interrupt to bring the microcontroller out of its low
* power state at the time the kernel next needs to execute.
* Enter the low power state.
*/
pm_sleep_enter( ulSleepTime );
}
#if (dg_configIMAGE_SETUP == DEVELOPMENT_MODE)
/* If the code stops at this point then the interrupts were enabled while they
* shouldn't be so.
*/
primask = __get_PRIMASK();
ASSERT_WARNING(primask == 1);
#endif
/* Wake-up! */
pm_system_wake_up();
}
}
void restore_global_interrupts()
{
taskENABLE_INTERRUPTS();
}
