/*----------------------------------------------------------------------------
* Thread 1: Send thread
*---------------------------------------------------------------------------*/
void send_thread (void const *argument) {
T_MEAS *mptr;
mptr = osPoolAlloc(mpool); /* Allocate memory for the message */
mptr->voltage = 223.72; /* Set the message content */
mptr->current = 17.54;
mptr->counter = 120786;
osMessagePut(MsgBox, (uint32_t)mptr, osWaitForever); /* Send Message */
osDelay(100);
mptr = osPoolAlloc(mpool); /* Allocate memory for the message */
mptr->voltage = 227.23; /* Prepare a 2nd message */
mptr->current = 12.41;
mptr->counter = 170823;
osMessagePut(MsgBox, (uint32_t)mptr, osWaitForever); /* Send Message */
osThreadYield(); /* Cooperative multitasking */
osDelay(100);
mptr = osPoolAlloc(mpool); /* Allocate memory for the message */
mptr->voltage = 229.44; /* Prepare a 3rd message */
mptr->current = 11.89;
mptr->counter = 237178;
osMessagePut(MsgBox, (uint32_t)mptr, osWaitForever); /* Send Message */
osDelay(100);
/* We are done here, exit this thread */
}
/**
* @brief Send a generic message which packages a command
* @param msgQ: Message Q to send the message to
* @param source: Where the message is being sent from
* @param command: The command to send
* @param timeout: How long the operating system must wait until the message is successfully placed in message Q msgQ
*/
void sendCommand(osMessageQId msgQ, msgSource_t source, msgCommand_t command, uint32_t timeout) {
uint8_t dataLength;
msg_genericMessage_t *messageTxPtr;
data_command_t *commandStructTxPtr;
// Allocate a block of memory in the global memory pool for the generic message
messageTxPtr = osPoolAlloc(genericMPool);
// Identify the size of the data that will be linked in based on the data struct used
dataLength = sizeof(data_command_t);
// Allocated memory for the data
commandStructTxPtr = pvPortMalloc(dataLength);
// Fill the data into the allocated memory block
commandStructTxPtr->messageType = MSG_TYPE_COMMAND;
commandStructTxPtr->command = command;
// Pass the pointer to the generic message
messageTxPtr->pData = commandStructTxPtr;
// Polpulate the generic message
messageTxPtr->mRsp = MRSP_HANDLE;
messageTxPtr->messageType = MSG_TYPE_COMMAND;
messageTxPtr->messageSource = source;
messageTxPtr->dataLength = dataLength;
// Send the message!
osMessagePut(msgQ, (uint32_t) messageTxPtr, timeout);
}
/**************************************************************************//**
* @brief
* Main function is a CMSIS RTOS thread in itself
*
* @note
* This example uses threads, memory pool and message queue to demonstrate the
* usage of these CMSIS RTOS features. In this simple example, the same
* functionality could more easily be achieved by doing everything in the main
* loop.
*****************************************************************************/
int main(void)
{
int count = 0;
/* Chip errata */
CHIP_Init();
/* Initialize CMSIS RTOS structures */
/* create memory pool */
mpool = osPoolCreate(osPool(mpool));
/* create msg queue */
msgBox = osMessageCreate(osMessageQ(msgBox), NULL);
/* create thread 1 */
osThreadCreate(osThread(PrintLcdThread), NULL);
/* Infinite loop */
while (1)
{
count = (count + 1) & 0xF;
/* Send message to PrintLcdThread */
/* Allocate memory for the message */
lcdText_t *mptr = osPoolAlloc(mpool);
/* Set the message content */
(*mptr)[0] = count >= 10 ? '1' : '0';
(*mptr)[1] = count % 10 + '0';
(*mptr)[2] = '\0';
/* Send message */
osMessagePut(msgBox, (uint32_t) mptr, osWaitForever);
/* Wait now for half a second */
osDelay(500);
}
}
/// Allocate a memory block from a mail
/// \param[in] queue_id mail queue ID obtained with \ref osMailCreate.
/// \param[in] millisec timeout value or 0 in case of no time-out
/// \return pointer to memory block that can be filled with mail or NULL in case error.
/// \note MUST REMAIN UNCHANGED: \b osMailAlloc shall be consistent in every CMSIS-RTOS.
void *osMailAlloc (osMailQId queue_id, uint32_t millisec)
{
(void) millisec;
void *p;
uint32_t retried=0;
if (queue_id == NULL) {
return NULL;
}
do {
p = osPoolAlloc(queue_id->pool);
if (NULL == p) {
// sleep a while and then try again
if (millisec == osWaitForever) {
osDelay(2);
}
else {
if (!retried) {
osDelay(millisec);
retried = 1;
}
else {
break;
}
}
}
} while (NULL == p);
return p;
}
/// Allocate a memory block from a memory pool and set memory block to zero
/// \param[in] pool_id memory pool ID obtain referenced with \ref osPoolCreate.
/// \return address of the allocated memory block or NULL in case of no memory available.
/// \note MUST REMAIN UNCHANGED: \b osPoolCAlloc shall be consistent in every CMSIS-RTOS.
void *osPoolCAlloc (osPoolId pool_id)
{
void *p = osPoolAlloc(pool_id);
os_memset(p, 0, pool_id->item_sz);
return p;
}
/**@brief Function for receiving the Application's BLE Stack events.
*
* @param[in] p_ble_evt Bluetooth stack event.
*/
static void on_ble_evt(ble_evt_t * p_ble_evt)
{
ble_evt_t * mptr;
mptr = osPoolAlloc(ble_evt_pool);
*mptr = *p_ble_evt;
(void)osMessagePut(ble_stack_msg_box, (uint32_t)mptr, 0);
}
/**
* @brief Allocate a memory block from a memory pool and set memory block to zero
* @param pool_id memory pool ID obtain referenced with \ref osPoolCreate.
* @retval address of the allocated memory block or NULL in case of no memory available.
* @note MUST REMAIN UNCHANGED: \b osPoolCAlloc shall be consistent in every CMSIS-RTOS.
*/
void *osPoolCAlloc (osPoolId pool_id)
{
void *p = osPoolAlloc(pool_id);
if (p != NULL)
{
memset(p, 0, sizeof(pool_id->pool_sz));
}
return p;
}
/**
* @brief Allocate a memory block from a mail
* @param queue_id mail queue ID obtained with \ref osMailCreate.
* @param millisec timeout value or 0 in case of no time-out.
* @retval pointer to memory block that can be filled with mail or NULL in case error.
* @note MUST REMAIN UNCHANGED: \b osMailAlloc shall be consistent in every CMSIS-RTOS.
*/
void *osMailAlloc (osMailQId queue_id, uint32_t millisec)
{
(void) millisec;
void *p;
if (queue_id == NULL) {
return NULL;
}
p = osPoolAlloc(queue_id->pool);
return p;
}
/**
* @brief Send a generic message which packages any data structure
* @param msgQ: Message Q to send the message to
* @param type: Type of data that is linked in
* @param source: Where the message is being sent from
* @param mRsp: Memory responsibility - whether or not the library should be responsible for allocating and deallocating the memory for the data
* @param *data: Pointer to the struct holding the message data
* @param timeout: How long the operating system must wait until the message is successfully placed in message Q msgQ
*/
void sendMessage(osMessageQId msgQ, msgType_t type, msgSource_t source, uint8_t mRsp, void *pData, uint32_t timeout) {
uint32_t dataLength; // Length of the data to be linked in
msg_genericMessage_t *messageTxPtr;
void *dataContentPtr;
// Allocate a block of memory in the global memory pool for the generic message
messageTxPtr = osPoolAlloc(genericMPool);
// Identify the size of the data that will be linked in based on the data struct used
switch (type) {
case MSG_TYPE_STRING:
dataLength = *((uint16_t *) pData) + sizeof(uint16_t);
break;
case MSG_TYPE_COMMAND:
dataLength = sizeof(data_command_t);
break;
case MSG_TYPE_COORDS:
dataLength = sizeof(data_coords_t);
break;
}
// If the library is held responsible for managing memory for data, do the thing!
if (mRsp) {
// Allocated memory for the data
dataContentPtr = pvPortMalloc(dataLength);
// Copy the data into the allocated memory block
memcpy(dataContentPtr, pData, dataLength);
// Pass the pointer to the generic message
messageTxPtr->pData = dataContentPtr;
messageTxPtr->mRsp = MRSP_HANDLE;
} else {
// Just pass the generic message the pointer, the library will not handle the memory
messageTxPtr->pData = pData;
messageTxPtr->mRsp = MRSP_NO_HANDLE;
}
// Polpulate the generic message
messageTxPtr->messageType = type;
messageTxPtr->messageSource = source;
messageTxPtr->dataLength = dataLength;
// Send the message!
osMessagePut(msgQ, (uint32_t) messageTxPtr, timeout);
}
/**************************************************************************//**
* @brief
* Main function is a CMSIS RTOS thread in itself
*
* @note
* This example uses threads, memory pool and message queue to demonstrate the
* usage of these CMSIS RTOS features. In this simple example, the same
* functionality could more easily be achieved by doing everything in the main
* loop.
*****************************************************************************/
int main(void)
{
int count = 0;
/* Chip errata */
CHIP_Init();
/* If first word of user data page is non-zero, enable eA Profiler trace */
BSP_TraceProfilerSetup();
#if 0
/* Reduce power consumption by disabling part of RAM */
/* this requires changing linker script to avoid placing */
/* data in RAM above 32kB. Blocks 1,2,3 are turn off. */
EMU_MemPwrDown(_EMU_MEMCTRL_POWERDOWN_BLK123);
BURTC->POWERDOWN |= BURTC_POWERDOWN_RAM; /* turn BURTC RAM off */
LESENSE->POWERDOWN |= LESENSE_POWERDOWN_RAM; /* turn off LESENSE RAM */
#endif
/* Initialize CMSIS RTOS structures */
/* create memory pool */
mpool = osPoolCreate(osPool(mpool));
/* create msg queue */
msgBox = osMessageCreate(osMessageQ(msgBox), NULL);
/* create thread 1 */
osThreadCreate(osThread(PrintLcdThread), NULL);
/* Infinite loop */
while (1)
{
count = (count + 1) & 0xF;
/* Send message to PrintLcdThread */
/* Allocate memory for the message */
lcdText_t *mptr = osPoolAlloc(mpool);
/* Set the message content */
(*mptr)[0] = count >= 10 ? '1' : '0';
(*mptr)[1] = count % 10 + '0';
(*mptr)[2] = '\0';
/* Send message */
osMessagePut(msgBox, (uint32_t) mptr, osWaitForever);
/* Wait now for half a second */
osDelay(500);
}
}
/**************************************************************************//**
* @brief
* Main function is a CMSIS RTOS thread in itself
*
* @note
* This example uses threads, memory pool and message queue to demonstrate the
* usage of these CMSIS RTOS features. In this simple example, the same
* functionality could more easily be achieved by doing everything in the main
* loop.
*****************************************************************************/
int main(void)
{
int count = 0;
/* Chip errata */
CHIP_Init();
/* If first word of user data page is non-zero, enable eA Profiler trace */
BSP_TraceProfilerSetup();
/* Initialize LED driver */
BSP_LedsInit();
/* Initialize the LCD driver */
SegmentLCD_Init(false);
/* Initialize CMSIS RTOS structures */
/* create memory pool */
mpool = osPoolCreate(osPool(mpool));
/* create msg queue */
msgBox = osMessageCreate(osMessageQ(msgBox), NULL);
/* create thread 1 */
osThreadCreate(osThread(PrintLcdThread), NULL);
/* Infinite loop */
while (1)
{
count = (count+1)&0xF;
BSP_LedsSet(count);
/* Send message to PrintLcdThread */
/* Allocate memory for the message */
lcdText_t *mptr = osPoolAlloc(mpool);
/* Set the message content */
(*mptr)[0] = count>=10 ? '1' : '0';
(*mptr)[1] = count%10 + '0';
(*mptr)[2] = '\0';
/* Send message */
osMessagePut(msgBox, (uint32_t)mptr, osWaitForever);
/* Wait now for half a second */
osDelay(500);
}
}
// Send the 'data' in the Message 'queue'
void send_message(float data, osMessageQId queue){
Message* msg = osPoolAlloc(mem_pool);
msg->data = data;
osMessagePut(queue, (uint32_t)msg, 0);
}
/**
* @brief System starts here and delegates different actions.
* @param None
* @retval None
*/
void acc_thread(void const *argument){
st_system_state state = ST_INIT ;
err_type error = NONE;
float pitchRadians, rollRadians, pitchDegrees, rollDegrees;
/* global variable */
int init_time = 0;
filterState accPitchFilter;
filterState accRollFilter;
float filteredPitch=0.0;
float filteredRoll=0.0;
/* Main function */
while (1) {
/* The system can be in multiple states, this is just good coding practice */
switch ( state ){
/* Initialization state */
case ST_INIT :
/**************************************/
/************ initialization **********/
/**************************************/
//acc_init();
acc_ext_init();
initializeFilter(&accPitchFilter, 35);
initializeFilter(&accRollFilter, 35);
/**************************************/
state = ST_IDLE ;
break ;
/* Idle State */
case ST_IDLE :
/**************************************/
/**************** idle ****************/
/**************************************/
delay(init_time);
/**************************************/
state = ST_RUNNING ;
break ;
/* The system is running here the majority of the time */
case ST_RUNNING :
/**************************************/
/***************** run ****************/
/**************************************/
/* Accelerometer Interrupt. When high run the following: */
//if(interrupt_EXTI0 == 1){
osSignalWait(0x1,osWaitForever);
/* Reset flag */
interrupt_EXTI0 = 0;
/* Read accelerometer data X,Y,Z into */
float data[3];
//LIS3DSH_ReadACC(data);
//LSM9DS1_ReadACC(data);
LSM9DS1_ReadGYRO(data);
offsetAccData(data);
//printf("%f\t%f\t%f\n", data[0]/*x*/, data[1]/*y*/, data[2]/*z*/);
/* Calculate the pitch and roll in radians based on the following equation: */
/* Pitch = arctan(Ax1/sqrt((Ay1)^2+(Az1)^2 */
/* Roll = arctan(Ay1/sqrt((Ax1)^2+(Az1)^2 */
pitchRadians = calculatePitch(data);
rollRadians = calculateRoll(data);
/* Converting pitch and roll to degrees */
pitchDegrees = convertToDegrees(pitchRadians);
rollDegrees = convertToDegrees(rollRadians);
/* Filtering the pitch and roll angles in degrees using a moving average filter*/
filteredPitch = modify_filterState(&accPitchFilter, pitchDegrees);
filteredRoll = modify_filterState(&accRollFilter, rollDegrees);
/* Create message */
message_acc* acc_message = (message_acc*) osPoolAlloc(acc_mpool);
acc_message->pitch = filteredPitch;
acc_message->roll = filteredRoll;
osMessagePut(acc_mqueue, (uint32_t)acc_message, osWaitForever);
printf("%lf\t%f\t%lf\t%f\n", pitchDegrees, filteredPitch, rollDegrees, filteredRoll);
//}
/**************************************/
break ;
case ST_ERROR :
/**************************************/
/**************** error ***************/
/**************************************/
switch ( error ){
case NONE :
//You didn't set error type...
break ;
case ERR_ERROR_1 :
break ;
case ERR_ERROR_2 :
break ;
case ERR_ERROR_3 :
break ;
case ERR_ERROR_4 :
break ;
}
/**************************************/
break ;
}
//osDelay(250);
}
}
