//-------------------------------------------------------------------
//read date time
void RTC_Read_datetime(uint8_t * data,uint8_t flag)
{
uint8_t temp[3];
//first read tiem ,then read date, or not time is not run;
RTC_DateTypeDef sdatestructureget;
RTC_TimeTypeDef stimestructureget;
HAL_RTCStateTypeDef status;
if(data!=NULL)
{
osMutexWait(rtc_mutex, osWaitForever);
/* Get the RTC current Time */
HAL_RTC_GetTime(&hrtc, &stimestructureget, RTC_FORMAT_BIN);
temp[0]=stimestructureget.Hours;
temp[1]=stimestructureget.Minutes;
temp[2]=stimestructureget.Seconds;
memcpy(¤t_datetime[3],temp,3);
/* Get the RTC current Date */
HAL_RTC_GetDate(&hrtc, &sdatestructureget, RTC_FORMAT_BIN);
data[0]=sdatestructureget.Year;
data[1]=sdatestructureget.Month;
data[2]=sdatestructureget.Date;
current_datetime[6]=sdatestructureget.WeekDay;
memcpy(¤t_datetime[0],data,3);
if(flag==1)
{
memcpy(data,temp,3);
}
osMutexRelease(rtc_mutex);
}
}
/**
\brief Test case: TC_MutexNestedAcquire
\details
- Create a mutex object
- Obtain a mutex object
- Create a high priority thread that waits for the same mutex
- Recursively acquire and release a mutex object
- Release a mutex
- Verify that every subsequent call released the mutex
- Delete a mutex object
- Mutex object must be released after each acquisition
*/
void TC_MutexNestedAcquire (void) {
osStatus stat;
/* - Create a mutex object */
G_MutexId = osMutexCreate (osMutex (Mutex_Nest));
ASSERT_TRUE (G_MutexId != NULL);
if (G_MutexId != NULL) {
/* - Obtain a mutex object */
stat = osMutexWait (G_MutexId, 0);
ASSERT_TRUE (stat == osOK);
if (stat == osOK) {
/* - Create a high priority thread that will wait for the same mutex */
G_Mutex_ThreadId = osThreadCreate (osThread (Th_MutexWait), NULL);
ASSERT_TRUE (G_Mutex_ThreadId != NULL);
/* - Recursively acquire and release a mutex object */
RecursiveMutexAcquire (5, 5);
/* - Release a mutex */
stat = osMutexRelease (G_MutexId);
ASSERT_TRUE (stat == osOK);
/* - Verify that every subsequent call released the mutex */
ASSERT_TRUE (osMutexRelease (G_MutexId) == osErrorResource);
}
/* - Delete a mutex object */
ASSERT_TRUE (osMutexDelete (G_MutexId) == osOK);
}
}
error_t udpInvokeRxCallback(NetInterface *interface, const IpPseudoHeader *pseudoHeader,
const UdpHeader *header, const ChunkedBuffer *buffer, size_t offset)
{
uint_t i;
void *params;
UdpRxCallbackDesc *entry;
//This flag tells whether a matching entry has been found
bool_t found = FALSE;
//Acquire exclusive access to the callback table
osMutexAcquire(udpCallbackMutex);
//Loop through the table
for(i = 0; i < UDP_CALLBACK_TABLE_SIZE; i++)
{
//Point to the current entry
entry = &udpCallbackTable[i];
//Check whether the entry is currently in used
if(entry->callback != NULL)
{
//Bound to a particular interface?
if(entry->interface == NULL || entry->interface == interface)
{
//Does the specified port number match the current entry?
if(entry->port == ntohs(header->destPort))
{
//Retrieve callback parameters
params = entry->params;
//Release mutex to prevent any deadlock
if(params == NULL)
osMutexRelease(udpCallbackMutex);
//Invoke user callback function
entry->callback(interface, pseudoHeader,
header, buffer, offset, params);
//Acquire mutex
if(params == NULL)
osMutexAcquire(udpCallbackMutex);
//A matching entry was found
found = TRUE;
}
}
}
}
//Release exclusive access to the callback table
osMutexRelease(udpCallbackMutex);
//Return status code
return found ? NO_ERROR : ERROR_PORT_UNREACHABLE;
}
error_t ipv4LeaveMulticastGroup(NetInterface *interface, Ipv4Addr groupAddr)
{
uint_t i;
uint_t j;
MacAddr macAddr;
//Ensure the specified IPv4 address is a multicast address
if(!ipv4IsMulticastAddr(groupAddr))
return ERROR_INVALID_ADDRESS;
//Acquire exclusive access to the IPv4 filter table
osMutexAcquire(interface->ipv4FilterMutex);
//Loop through filter table entries
for(i = 0; i < interface->ipv4FilterSize; i++)
{
//Specified IPv4 address found?
if(interface->ipv4Filter[i].addr == groupAddr)
{
//Decrement the reference count
interface->ipv4Filter[i].refCount--;
//Remove the entry if the reference count drops to zero
if(interface->ipv4Filter[i].refCount < 1)
{
#if (IGMP_SUPPORT == ENABLED)
//Report group membership termination
igmpLeaveGroup(interface, &interface->ipv4Filter[i]);
#endif
//Map the multicast IPv4 address to a MAC-layer address
ipv4MapMulticastAddrToMac(groupAddr, &macAddr);
//Drop the corresponding address from the MAC filter table
ethDropMulticastAddr(interface, &macAddr);
//Adjust the size of the IPv4 filter table
interface->ipv4FilterSize--;
//Remove the corresponding entry
for(j = i; j < interface->ipv4FilterSize; j++)
interface->ipv4Filter[j] = interface->ipv4Filter[j + 1];
}
//Release exclusive access to the IPv4 filter table
osMutexRelease(interface->ipv4FilterMutex);
//No error to report
return NO_ERROR;
}
}
//Release exclusive access to the IPv4 filter table
osMutexRelease(interface->ipv4FilterMutex);
//The specified IPv4 address does not exist
return ERROR_FAILURE;
}
/**
* @brief Function that receives data and performs Triple Modular Redundancy. This is a blocking call.
* @param *receiver: pointer to a Receiver structure.
* @retval None.
*/
void wireless_RX(struct Receiver *receiver) {
uint8_t i=0;
uint8_t temp_data=0;
osMutexWait(receiver->mutexID, osWaitForever);
uint8_t raw_data[sizeof(receiver->data)/sizeof(receiver->data[0]) * 3];
CC2500_StrobeSend(SRX_R,&(receiver->state),&(receiver->buffer_space));
osMutexRelease(receiver->mutexID);
osDelay(STROBE_DELAY);
while (i<(sizeof(receiver->data)/sizeof(receiver->data[0]) * 3)) {
osMutexWait(receiver->mutexID, osWaitForever);
CC2500_StrobeSend(SNOP_R,&(receiver->state),&(receiver->buffer_space));
if (receiver->buffer_space>0) {
CC2500_Read(&temp_data, 0x3F, 1);
if ((temp_data&0xF0)==0xF0) {
raw_data[0]=temp_data&0x0F;
i=1;
} else if (i>0) {
if ((temp_data&0xF0)==i<<4) {
raw_data[i]=temp_data&0x0F;
i++;
} else {
i=0;
}
}
}
osMutexRelease(receiver->mutexID);
osDelay(STROBE_DELAY);
}
osMutexWait(receiver->mutexID, osWaitForever);
for(uint32_t j=0;j<sizeof(receiver->data)/sizeof(receiver->data[0]);j++){
receiver->data[j] = ((raw_data[3*j]&raw_data[3*j+1]) | (raw_data[3*j]&raw_data[3*j+2]) | (raw_data[3*j+2]&raw_data[3*j+1]));
}
CC2500_StrobeSend(SIDLE_R,&(receiver->state),&(receiver->buffer_space));
osMutexRelease(receiver->mutexID);
osDelay(STROBE_DELAY);
osMutexWait(receiver->mutexID, osWaitForever);
CC2500_StrobeSend(SNOP_R,&(receiver->state),&(receiver->buffer_space));
osMutexRelease(receiver->mutexID);
osDelay(STROBE_DELAY);
}
void Thread_Mutex(void const *argument) {
osStatus status;
while(1) {
; // Insert thread code here...
status = osMutexWait(mid_Thread_Mutex, NULL);
switch(status) {
case osOK:
; // Use protected code here...
osMutexRelease(mid_Thread_Mutex);
break;
case osErrorTimeoutResource:
break;
case osErrorResource:
break;
case osErrorParameter:
break;
case osErrorISR:
break;
default:
break;
}
osThreadYield(); // suspend thread
}
}
//Send data in specified format. Same syntax as printf
util_ErrTd xb_SendF(char *format, ...){
util_ErrTd Status = util_ErrTd_Ok;
va_list va;
int32_t i;
if( osMutexWait(xb_MutexId, 100) == osOK ){ //Wait for shared resource (tx buffer access)
HAL_NVIC_DisableIRQ(XB_DMA_TX_IRQN); //Disable DMA interrupts (TXC could acces TxBuffer in the middle of writing data to it
va_start(va, format); //Start reading of parameters
vsnprintf(xb_TmpStr, sizeof(xb_TmpStr), format, va); //Format new string and save its length
va_end(va); //End of reading parameters
// if( Cnt <= 0 || Cnt >= sizeof(com_TmpStr) ){ //If formatted string doesnt fit into buffer
// snprintf(com_TmpStr, sizeof(com_TmpStr), "<erre %d %d>\r\n", (int)Cnt, (int)sizeof(com_TmpStr)); //Format error message instead
// Status = util_ErrTd_Overflow;
// }
for( i=0; i<strlen(xb_TmpStr); i++ ){ //Copy byte by byte into tx buffer
xb_TxBuffer[ xb_TxHead++ ] = xb_TmpStr[i];
xb_TxHead &= XB_TXBUFHEADMASK;
}
HAL_NVIC_EnableIRQ(XB_DMA_TX_IRQN); //Enable DMA interrupts again
xb_TransmitTxBuffer(); //Transmit TX buffer
osMutexRelease(xb_MutexId); //Release shared resource
}
return Status;
}
void memPoolFree(void *p)
{
//Use fixed-size blocks allocation?
#if (MEM_POOL_SUPPORT == ENABLED)
uint_t i;
//Acquire exclusive access to the memory pool
osMutexAcquire(memPoolMutex);
//Loop through allocation table
for(i = 0; i < MEM_POOL_BUFFER_COUNT; i++)
{
if(memPool[i] == p)
{
//Mark the current block as free
memPoolAllocTable[i] = FALSE;
//Exit immediately
break;
}
}
//Release exclusive access to the memory pool
osMutexRelease(memPoolMutex);
#else
//Release memory block
osMemFree(p);
#endif
}
/**
* @brief Set value to be displayed on the seven-segment display.
* @param float angle: angle to be displayed
* @retval None
*/
void SevenSegment_SetDisplayValue_Angle(float angle)
{
/* We have race conditions. Need to use mutexes to protect these global flags. */
osMutexWait(segment_mutex, osWaitForever);
displayed_angle = angle;
osMutexRelease(segment_mutex);
}
/**
* @brief Set value to be displayed on the seven-segment display.
* @param float angle: angle to be displayed
* @retval None
*/
void SevenSegment_SetDisplayValue_Temp(float temp)
{
/* We have race conditions. Need to use mutexes to protect these global flags. */
osMutexWait(segment_mutex, osWaitForever);
displayed_temp = temp;
osMutexRelease(segment_mutex);
}
/*----------------------------------------------------------------------------
* Thread 'SEGMENT': Display values on 7-segment display
*---------------------------------------------------------------------------*/
void Thread_SEGMENT (void const *argument)
{
int counter = 0;
DisplayMode mode;
while(1)
{
osSignalWait(SEGMENT_SIGNAL, osWaitForever);
if (counter % FLASH_PERIOD == 0)
{
if (SevenSegment_GetFlashing()) {
osMutexWait(segment_mutex, osWaitForever);
activated = !activated;
osMutexRelease(segment_mutex);
}
}
mode = SevenSegment_GetDisplayMode();
if (mode == TEMP_MODE) {
SevenSegment_ToggleDisplayedDigit_Angle();
} else if (mode == ANGLE_MODE) {
SevenSegment_ToggleDisplayedDigit_Temp();
}
counter++;
}
}
/**
* @brief Sets seven-segment display mode (ANGLE_MODE, TEMP_MODE).
* @param None
* @retval None
*/
void SevenSegment_SetDisplayMode(DisplayMode mode)
{
/* We have race conditions. Need to use mutexes to protect these global flags. */
osMutexWait(segment_mutex, osWaitForever);
display_mode = mode;
osMutexRelease(segment_mutex);
}
/**
* @brief Starts flashing the display.
* @param None
* @retval None
*/
void SevenSegment_StartFlashing(void)
{
/* We have race conditions. Need to use mutexes to protect these global flags. */
osMutexWait(segment_mutex, osWaitForever);
flashing = 1;
osMutexRelease(segment_mutex);
}
/**
* @brief Deactivates 7-segment display and turn off the LEDs.
* @param None
* @retval None
*/
void SevenSegment_TurnOff(void)
{
osMutexWait(segment_mutex, osWaitForever);
GPIOE->ODR &= 0x000F; /* Clear the bits corresponding to GPIO_PIN_4 to GPIO_PIN_15 */
activated = 0;
osMutexRelease(segment_mutex);
}
/**
* @brief Activates 7-segment display.
* @param None
* @retval None
*/
void SevenSegment_TurnOn(void)
{
/* We have race conditions. Need to use mutexes to protect these global flags. */
osMutexWait(segment_mutex, osWaitForever);
activated = 1;
osMutexRelease(segment_mutex);
}
/*-----------------------------------------------------------------------------
* Low priority job used for priority inversion test
*----------------------------------------------------------------------------*/
void Th_LowPrioJob (void const *arg) {
osThreadId *ctrl_id = (osThreadId *)arg;
osStatus stat;
uint32_t i;
/* Obtain a mutex object */
stat = osMutexWait (G_MutexId, 0);
ASSERT_TRUE (stat == osOK);
if (stat == osOK) {
/* Mutex acquired, inform control thread */
osSignalSet (*ctrl_id, (1 << 0));
/* Set mark into execution array */
for (i = 0; i < 3; i++) {
if (G_ExecArr[i] == 0) {
G_ExecArr[i] = 'L'; /* L as Low priority job */
/* Inform control thread */
osSignalSet (*ctrl_id, (1 << 1));
break;
}
}
ASSERT_TRUE (osMutexRelease (G_MutexId) == osOK);
}
}
/* State behaviour */
void behaviour_welcome(state_ptr state)
{
/* Set events to react to */
/* Do state actions */
/* Set menu */
osMutexWait(mutex_menuHandle, osWaitForever);
menu_copy(&menu_welcome, ¤t_menu);
osMutexRelease(mutex_menuHandle);
/* Display menu */
uint32_t i;
for (i = 0; i < menu_welcome.item_num; i++)
{
while (osMailPut(queue_lcdHandle, (void *) &menu_welcome.items[i]) != osOK)
{
osDelay(1);
}
}
/* Do state actions */
bluetooth_init();
osDelay(2500);
entry_to_running(state);
}
bool spifs_unlink(const char *filename)
{
int16_t fileId;
char cleanname[SPIFS_FILENAME_LEN];
SPIFlash handler;
// Clean the filename of problem characters.
if (!clean_filename(filename, cleanname)) return false;
// Wait for exclusive access to the SPI master bus.
osMutexWait(spiMasterMutex, osWaitForever);
// Open the SPI flash device.
spi_flash_open(SPI0, SPI_MODE0, false, Freq_1Mbps, &handler);
// Find a file id associated with the filename.
fileId = spifs_find_filename(&handler, cleanname);
// Erase each fileblock associated with the file.
if (SPIFS_FILEID_VALID(fileId)) spifs_erase_blocks(&handler, fileId);
// Close the SPI flash device.
spi_flash_close(&handler);
// Release exclusive access to the SPI master bus.
osMutexRelease(spiMasterMutex);
return true;
}
/*-----------------------------------------------------------------------------
* Recursive mutex acquisition
*----------------------------------------------------------------------------*/
static void RecursiveMutexAcquire (uint32_t depth, uint32_t ctrl) {
static uint32_t acq; /* Mutex acquisition counter */
osStatus stat;
/* Acquire a mutex */
stat = osMutexWait (G_MutexId, 100);
ASSERT_TRUE (stat == osOK);
if (stat == osOK) {
if (ctrl == depth) {
/* - Verify that mutex was aqcuired at count zero */
ASSERT_TRUE (acq == 0);
}
acq++;
if (depth) {
RecursiveMutexAcquire (depth - 1, ctrl);
}
acq--;
/* Release a mutex */
stat = osMutexRelease (G_MutexId);
ASSERT_TRUE (stat == osOK);
}
}
/*---------------------------------------------------------------------------
TITLE : cmd_bluetooth_check
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
void cmd_bluetooth_check( void )
{
uint32_t time_out;
uint8_t ch;
uint8_t ch_array[2];
uint8_t ch_i;
osStatus ret;
ret = osMutexWait( Mutex_Loop, 1000 );
if( ret != osOK )
{
_menu_printf("Fail to osMutexWait\r\n");
return;
}
core.blueport = uartOpen(USART2, NULL, 115200, MODE_RXTX);
_menu_printf("\r\n");
_menu_printf("AT -> ");
serialPrint(core.blueport, "AT");
ch_array[0] = 0;
ch_array[1] = 0;
ch_i = 0;
//-- 응답이 올때까지 기다림
time_out = 1000;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
_menu_putch(ch);
ch_array[ch_i++] = ch;
if( ch_i >= 2 ) break;
}
osDelay(1);
}
if( ch_array[0] == 'O' && ch_array[1] == 'K' )
{
_menu_printf("\r\nBluetooth OK");
}
else
{
_menu_printf("\r\nBluetooth Fail");
}
_menu_printf("\r\n");
serialInit(mcfg.serial_baudrate);
osMutexRelease( Mutex_Loop );
}
void Thread_DISP2(void const *argument){
float roll_temp, pitch_temp;
while(1){
// osDelay(10);
osMutexWait(mems_mutex_id, osWaitForever);
roll_temp = roll;
osMutexRelease(mems_mutex_id);
// printf("mems: roll= %f\n", roll_temp);
printf("mems: temp= %f\n", output);
if(roll_temp < 100){
Parse_Mems(parsed, roll_temp);
// printf("disp2: %d %d %d %d %d\n", parsed[3], parsed[2], parsed[1], parsed[0], parsed[4]);
}
else{
parsed[0] = (int) roll_temp % 10;
parsed[2] = ((int) roll_temp / 10) % 10;
parsed[3] = ((int) roll_temp / 100) % 10;
parsed[1] = 0;
parsed[4] = 0;
// printf("disp2: %d %d %d %d %d\n", parsed[3], parsed[2], parsed[1], parsed[0], parsed[4]);
}
if(parsed[4] < 0 || parsed[3] < 0 || parsed[2] < 0 || parsed[1] < 0 || parsed[0] < 0) Show_Negative();
else Show();
}
}
void ipv6FragTick(NetInterface *interface)
{
error_t error;
uint_t i;
time_t time;
Ipv6HoleDesc *hole;
//Acquire exclusive access to the reassembly queue
osMutexAcquire(interface->ipv6FragQueueMutex);
//Get current time
time = osGetTickCount();
//Loop through the reassembly queue
for(i = 0; i < IPV6_MAX_FRAG_DATAGRAMS; i++)
{
//Point to the current entry in the reassembly queue
Ipv6FragDesc *frag = &interface->ipv6FragQueue[i];
//Make sure the entry is currently in use
if(frag->buffer.chunkCount > 0)
{
//If the timer runs out, the partially-reassembled datagram must be
//discarded and ICMPv6 Time Exceeded message sent to the source host
if((time - frag->timestamp) >= IPV6_FRAG_TIME_TO_LIVE)
{
//Debug message
TRACE_INFO("IPv6 fragment reassembly timeout...\r\n");
//Dump IP header contents for debugging purpose
ipv6DumpHeader(frag->buffer.chunk[0].address);
//Point to the first hole descriptor
hole = ipv6FindHole(frag, frag->firstHole);
//Make sure the fragment zero has been received
//before sending an ICMPv6 message
if(hole != NULL && hole->first > 0)
{
//Fix the size of the reconstructed datagram
error = chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer,
frag->unfragPartLength + hole->first);
//Check status code
if(!error)
{
//Send an ICMPv6 Time Exceeded message
icmpv6SendErrorMessage(interface, ICMPV6_TYPE_TIME_EXCEEDED,
ICMPV6_CODE_REASSEMBLY_TIME_EXCEEDED, 0, (ChunkedBuffer *) &frag->buffer);
}
}
//Drop the partially reconstructed datagram
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
}
}
}
//Release exclusive access to the reassembly queue
osMutexRelease(interface->ipv6FragQueueMutex);
}
void tcpIpStackRxTask(void *param)
{
//Point to the structure describing the network interface
NetInterface *interface = (NetInterface *) param;
//Main loop
while(1)
{
//Receive notifications when a Ethernet frame has been received,
//or the link status has changed
osEventWait(interface->nicRxEvent, INFINITE_DELAY);
//Get exclusive access to the device
osMutexAcquire(interface->nicDriverMutex);
//Disable Ethernet controller interrupts
interface->nicDriver->disableIrq(interface);
//Handle incoming packets and link state changes
interface->nicDriver->rxEventHandler(interface);
//Re-enable Ethernet controller interrupts
interface->nicDriver->enableIrq(interface);
//Release exclusive access to the device
osMutexRelease(interface->nicDriverMutex);
}
}
error_t udpReceiveDatagram(Socket *socket, IpAddr *srcIpAddr, uint16_t *srcPort,
IpAddr *destIpAddr, void *data, size_t size, size_t *received, uint_t flags)
{
SocketQueueItem *queueItem;
//The receive queue is empty?
if(!socket->receiveQueue)
{
//Set the events the application is interested in
socket->eventMask = SOCKET_EVENT_RX_READY;
//Reset the event object
osEventReset(socket->event);
//Leave critical section
osMutexRelease(socketMutex);
//Wait until an event is triggered
osEventWait(socket->event, socket->timeout);
//Enter critical section
osMutexAcquire(socketMutex);
}
//Check whether the read operation timed out
if(!socket->receiveQueue)
{
//No data can be read
*received = 0;
//Report a timeout error
return ERROR_TIMEOUT;
}
//Point to the first item in the receive queue
queueItem = socket->receiveQueue;
//Copy data to user buffer
*received = chunkedBufferRead(data, queueItem->buffer, queueItem->offset, size);
//Save the source IP address
if(srcIpAddr)
*srcIpAddr = queueItem->srcIpAddr;
//Save the source port number
if(srcPort)
*srcPort = queueItem->srcPort;
//Save the destination IP address
if(destIpAddr)
*destIpAddr = queueItem->destIpAddr;
//If the SOCKET_FLAG_PEEK flag is set, the data is copied
//into the buffer but is not removed from the input queue
if(!(flags & SOCKET_FLAG_PEEK))
{
//Remove the item from the receive queue
socket->receiveQueue = queueItem->next;
//Deallocate memory buffer
chunkedBufferFree(queueItem->buffer);
}
//Update the state of events
udpUpdateEvents(socket);
//Successful read operation
return NO_ERROR;
}
void *memPoolAlloc(size_t size)
{
#if (MEM_POOL_SUPPORT == ENABLED)
uint_t i;
#endif
//Pointer to the allocated memory block
void *p = NULL;
//Debug message
TRACE_DEBUG("Allocating %" PRIuSIZE " bytes...\r\n", size);
//Use fixed-size blocks allocation?
#if (MEM_POOL_SUPPORT == ENABLED)
//Acquire exclusive access to the memory pool
osMutexAcquire(memPoolMutex);
//Enforce block size
if(size <= MEM_POOL_BUFFER_SIZE)
{
//Loop through allocation table
for(i = 0; i < MEM_POOL_BUFFER_COUNT; i++)
{
//Check whether the current block is free
if(!memPoolAllocTable[i])
{
//Mark the current entry as used
memPoolAllocTable[i] = TRUE;
//Point to the corresponding memory block
p = memPool[i];
//Update statistics
memPoolCurrentUsage++;
//Maximum number of buffers that have been allocated so far
memPoolMaxUsage = max(memPoolCurrentUsage, memPoolMaxUsage);
//Exit immediately
break;
}
}
}
//Release exclusive access to the memory pool
osMutexRelease(memPoolMutex);
#else
//Allocate a memory block
p = osMemAlloc(size);
#endif
//Failed to allocate memory?
if(!p)
{
//Debug message
TRACE_WARNING("Memory allocation failed!\r\n");
}
//Return a pointer to the allocated memory block
return p;
}
void mldProcessListenerReport(NetInterface *interface, Ipv6PseudoHeader *pseudoHeader,
const ChunkedBuffer *buffer, size_t offset, uint8_t hopLimit)
{
uint_t i;
size_t length;
MldMessage *message;
Ipv6FilterEntry *entry;
//Retrieve the length of the MLD message
length = chunkedBufferGetLength(buffer) - offset;
//The message must be at least 24 octets long
if(length < sizeof(MldMessage))
return;
//Point to the beginning of the MLD message
message = chunkedBufferAt(buffer, offset);
//Sanity check
if(!message) return;
//Debug message
TRACE_INFO("MLD message received (%" PRIuSIZE " bytes)...\r\n", length);
//Dump message contents for debugging purpose
mldDumpMessage(message);
//Make sure the source address of the message is a valid link-local address
if(!ipv6IsLinkLocalUnicastAddr(&pseudoHeader->srcAddr))
return;
//Check the Hop Limit field
if(hopLimit != MLD_HOP_LIMIT)
return;
//Acquire exclusive access to the IPv6 filter table
osMutexAcquire(interface->ipv6FilterMutex);
//Loop through filter table entries
for(i = 0; i < interface->ipv6FilterSize; i++)
{
//Point to the current entry
entry = &interface->ipv6Filter[i];
//Report messages are ignored for multicast addresses
//in the Non-Listener or Idle Listener state
if(entry->state == MLD_STATE_DELAYING_LISTENER)
{
//The Multicast Listener Report message matches the current entry?
if(ipv6CompAddr(&message->multicastAddr, &entry->addr))
{
//Clear flag
entry->flag = FALSE;
//Switch to the Idle Listener state
entry->state = MLD_STATE_IDLE_LISTENER;
}
}
}
//Release exclusive access to the IPv6 filter table
osMutexRelease(interface->ipv6FilterMutex);
}
void eventOS_scheduler_mutex_release(void)
{
owner_count--;
if (0 == owner_count) {
event_mutex_owner_id = NULL;
}
osMutexRelease(event_mutex_id);
}
/*----------------------------------------------------------------------------
switch LED off
*---------------------------------------------------------------------------*/
void LED_off (unsigned char led) {
LED_Off(led);
osMutexWait(mut_GLCD, osWaitForever);
GLCD_SetBackColor(White);
GLCD_SetTextColor(Green);
GLCD_DisplayChar(4, 5+led, __FI, 0x80+0); /* Circle Empty */
osMutexRelease(mut_GLCD);
}
/**
* @brief Worker thread main superloop that defines how the thread will always display values
* depending on the relevant mode of operation
* @param void*
* @retval void
*/
void Thread_MEMS(void const *argument){
float read_acc[] = {0, 0, 0};
double output_x, output_y, output_z;
Reset_MEMS(&kstate_x);
Reset_MEMS(&kstate_y);
Reset_MEMS(&kstate_z);
// double den_pitch, den_roll;
while(1){
osDelay(200);
// printf("mems: works\n\r");
// printf("mems: interrupt = %d\n\r", interrupt);
if(interrupt != 0){
interrupt = 0;
// printf("mems: now interrupt = %d\n\r", interrupt);
LIS3DSH_ReadACC(read_acc);
// printf("%f | %f | %f\n", read_acc[0], read_acc[1], read_acc[2]);
if(!Kalmanfilter_C(read_acc[0], &output_x, &kstate_x) && !Kalmanfilter_C(read_acc[1], &output_y, &kstate_y)
&& !Kalmanfilter_C(read_acc[2], &output_z, &kstate_z)){
// printf("x = %f out = %f\n", read_acc[0], output_x);
// printf("y = %f out = %f\n", read_acc[1], output_y);
// printf("z = %f out = %f\n", read_acc[2], output_z);
output_x = output_x + 0.10493;
output_y = output_y + 0.143217;
output_z = output_z + 0.665265;
osMutexWait(mems_mutex_id, osWaitForever);
// roll = (atan2(-fYg, fZg)*180.0)/M_PI;
roll = (atan2(-output_y, output_z) * 180.0) / 3.1416;
osMutexRelease(mems_mutex_id);
// printf("mems: roll= %f\n", roll);
osMutexWait(mems_mutex_id, osWaitForever);
// pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;
pitch = (atan2(output_x, sqrt(output_y * output_y + output_z * output_z))*180.0) / 3.1416;
osMutexRelease(mems_mutex_id);
// printf("mems: roll= %f\n", pitch);
}
else printf("mems: Kalman filter returned error!\n");
}
}
}
/*----------------------------------------------------------------------------
* Thread 'LED_Thread': Toggles LED
*---------------------------------------------------------------------------*/
void Thread_keypad (void const *argument) {
while(1){
osMutexWait(mutex, 100);
keypad_value = read_keypad();
osMutexRelease(mutex);
osDelay(1000);
}
}