Page MenuHomec4science
Files F91036976

communication.c
No OneTemporary
Actions

File Metadata

Created
Thu, Nov 7, 04:19

communication.c
View Options

#include "communication.h"
#include "controller.h"
#include "drivers/callback_timers.h"
#include "drivers/adc.h"
#include "drivers/dac.h"
#include "drivers/incr_encoder.h"
#include "drivers/hall.h"
#include "drivers/strain_gauge.h"
#include "drivers/tachometer.h"
#include "drivers/uart.h"
#include "lib/basic_filter.h"
#include "lib/pid.h"
#include "lib/utils.h"
#include <stdio.h>
uint32_t selectedVariablesToStream; // Bitfield that indicates for each variable if it should be streamed or not.
uint8_t txBuffer[1024];
#define COMM_BUFFER_SIZE 1024
#define DEBUG_MESSAGE_BUFFER_SIZE 1024
uint8_t comm_packetTxBuffer[COMM_BUFFER_SIZE];
char comm_debugMessageBuffer[DEBUG_MESSAGE_BUFFER_SIZE];
cb_CircularBuffer *comm_rxQueue;
uint8_t rxCurrentMessageType = PC_MESSAGE_DO_NOTHING; // Current message type for RX bytes.
uint32_t rxBytesCount; // Number of received bytes for the current message.
uint8_t firstHalfByte; // First half of the data byte to receive.
uint8_t rxDataBytesBuffer[32]; // Data bytes received (ready to use, bytes already merged).
// SyncVar-related vars.
#define N_SYNCVARS_MAX 50
comm_SyncVar comm_syncVars[N_SYNCVARS_MAX];
uint8_t comm_nSyncVars;
volatile bool comm_varListLocked;
uint8_t comm_streamId;
uint8_t comm_nVarsToStream;
comm_SyncVar const* comm_streamedVars[N_SYNCVARS_MAX];
extern volatile uint32_t ctrl_timestamp; // [us].
// Private functions.
void comm_SendPacket(uint8_t messageType, uint8_t *data, uint16_t dataLength);
void comm_HandleByte(uint8_t rxData);
void comm_Stream(void);
uint32_t comm_GetVar(uint8_t variableIndex);
void comm_SendVarsList(void);
/**
* @brief Init the communication manager.
*/
void comm_Init(void)
{
comm_nSyncVars = 0;
comm_varListLocked = false;
comm_streamId = 0;
comm_nVarsToStream = 0;
// Setup the UART peripheral, and specify the function that will be called
// each time a byte is received.
uart_Init();
comm_rxQueue = uart_GetRxQueue();
rxCurrentMessageType = PC_MESSAGE_DO_NOTHING;
// Make the streaming function periodically called by the timer 7.
cbt_SetCommLoopTimer(comm_Stream, TE_DATA_LOOP_DEFAULT_VAL);
}
void comm_Step(void)
{
// Send the bytes in the TX queue, even if it is not full, to avoid latency.
uart_FlushTx();
// Interpret the bytes in the RX queue.
uart_Step();
while(!cb_IsEmpty(comm_rxQueue))
comm_HandleByte(cb_Pull(comm_rxQueue));
}
void comm_NotifyReady(void)
{
comm_SendPacket(STM_MESSAGE_START_INFO, NULL, 0);
}
/**
* @brief UART data streaming "loop" function.
*/
void comm_Stream()
{
// If the data streaming is enabled, send a stream packet to the PC.
if(comm_nVarsToStream > 0)
{
uint8_t i;
int nDataBytesToSend = 0;
// Stream ID.
txBuffer[nDataBytesToSend] = comm_streamId;
nDataBytesToSend++;
// Timestamp.
memcpy(&txBuffer[nDataBytesToSend], (uint32_t*)&ctrl_timestamp,
sizeof(ctrl_timestamp));
nDataBytesToSend += sizeof(ctrl_timestamp);
// SyncVars values.
for(i=0; i<comm_nVarsToStream; i++)
{
comm_SyncVar const* sv = comm_streamedVars[i];
memcpy(&txBuffer[nDataBytesToSend], sv->address, sv->size);
nDataBytesToSend += sv->size;
}
comm_SendPacket(STM_MESSAGE_STREAMING_PACKET, txBuffer, nDataBytesToSend);
}
}
/**
* @brief Sends Packet with a header and data bytes.
* @param type: message type ("header" of the message).
* @param data: array of data bytes to be sent ("content" of the message).
* @param dataLength: number of data bytes to be sent.
*/
void comm_SendPacket(uint8_t type, uint8_t *data, uint16_t dataLength)
{
int i=0;
uint8_t *p = comm_packetTxBuffer;
*p = ((1<<7) | type);
p++;
for(i=0; i<dataLength; i++)
{
// Every data byte is splitted into two transmitted bytes.
*p = ((data[i]&0xf0) >> 4); // Most significant bits.
p++;
*p = ((data[i]&0x0f)); // Least significant bits.
p++;
}
uart_SendBytesAsync(comm_packetTxBuffer, dataLength*2+1);
}
/**
* @brief Process the received byte to interpret the messages.
* @param rxData: the byte to be processed and interpreted.
*/
void comm_HandleByte(uint8_t rxData)
{
if(rxData & (1<<7)) // The start byte has the most significant bit high.
{
rxCurrentMessageType = (rxData & ~(1<<7)); // Remove the start bit.
rxBytesCount = 0;
}
else
rxBytesCount++;
if(rxBytesCount % 2 == 1) // First half of the data byte has been received.
firstHalfByte = rxData; // Store it until the second half arrives.
else // Second half of the data byte has been received (or no data bytes yet).
{
int dataBytesReady = rxBytesCount/2;
rxDataBytesBuffer[dataBytesReady-1] = (firstHalfByte<<4) + (rxData & 0xf);
switch(rxCurrentMessageType)
{
case PC_MESSAGE_DO_NOTHING:
if(dataBytesReady == 0)
; // Do nothing.
break;
case PC_MESSAGE_PING:
if(dataBytesReady == 0)
comm_SendPacket(STM_MESSAGE_PINGBACK, NULL, 0);
break;
case PC_MESSAGE_GET_VARS_LIST:
if(dataBytesReady == 0 && comm_varListLocked)
{
int16_t i, length;
uint8_t *p = &txBuffer[0];
*p = (uint8_t)comm_nSyncVars;
p++;
for(i=0; i<comm_nSyncVars; i++)
{
memcpy(p, comm_syncVars[i].name, SYNCVAR_NAME_SIZE);
p += SYNCVAR_NAME_SIZE;
*p = (uint8_t)comm_syncVars[i].type;
p++;
*p = (uint8_t)comm_syncVars[i].access;
p++;
*p = (uint8_t)comm_syncVars[i].size;
p++;
}
length = p - &txBuffer[0];
comm_SendPacket(STM_MESSAGE_VARS_LIST, txBuffer, length);
}
break;
case PC_MESSAGE_GET_VAR:
if(dataBytesReady == 1)
{
comm_SyncVar *v;
// Extract the index, that indicates which variable will be sent.
uint8_t variableIndex = rxDataBytesBuffer[0];
// If the variable index is out of range, ignore the request.
if(variableIndex >= comm_nSyncVars)
return;
v = &comm_syncVars[variableIndex];
// If the variable is not readable, ignore the request.
if((v->usesVarAddress && v->access == WRITEONLY) ||
(!v->usesVarAddress && v->getFunc == NULL))
{
break;
}
// Prepare the message to be sent to the PC.
// First byte: variable index.
txBuffer[0] = variableIndex;
// Following bytes: actual value of the variable.
if(v->usesVarAddress)
{
v = &comm_syncVars[variableIndex];
memcpy(&txBuffer[1], v->address, v->size);
}
else
{
switch(v->type)
{
case BOOL:
{
bool tmpBool = ((bool (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpBool, v->size);
}
break;
case UINT8:
{
uint8_t tmpUint8 = ((uint8_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpUint8, v->size);
}
break;
case INT8:
{
int8_t tmpInt8 = ((int8_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpInt8, v->size);
}
break;
case UINT16:
{
uint16_t tmpUint16 = ((uint16_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpUint16, v->size);
}
break;
case INT16:
{
int16_t tmpInt16 = ((int16_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpInt16, v->size);
}
break;
case UINT32:
{
uint32_t tmpUint32 = ((uint32_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpUint32, v->size);
}
break;
case INT32:
{
int32_t tmpInt32 = ((int32_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpInt32, v->size);
}
break;
case UINT64:
{
uint32_t tmpUint64 = ((uint64_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpUint64, v->size);
}
break;
case INT64:
{
int32_t tmpInt64 = ((int64_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpInt64, v->size);
}
break;
case FLOAT32:
{
float32_t tmpFloat = ((float32_t (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpFloat, v->size);
}
break;
case FLOAT64:
{
double tmpDouble = ((double (*)(void))v->getFunc)();
memcpy(&txBuffer[1], &tmpDouble, v->size);
}
break;
}
}
// Send the message to the PC.
comm_SendPacket(STM_MESSAGE_VAR, txBuffer, 1 + v->size);
}
break;
case PC_MESSAGE_SET_VAR:
if(dataBytesReady >= 1)
{
comm_SyncVar *v;
uint8_t variableIndex;
// Extract the index (that indicates which variable will change),
// and the new value of the variable.
variableIndex = rxDataBytesBuffer[0];
// If the variable index is out of range, ignore the request.
if(variableIndex >= comm_nSyncVars)
return;
v = &comm_syncVars[variableIndex];
// Set the selected variable with the new value.
if(dataBytesReady == 1 + v->size)
{
if(v->usesVarAddress)
{
if(v->access != READONLY)
{
v = &comm_syncVars[variableIndex];
memcpy(v->address, &rxDataBytesBuffer[1], v->size);
}
}
else
{
if(v->setFunc == NULL)
break;
switch(v->type)
{
case BOOL:
{
bool tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(bool))v->setFunc)(tmp);
}
break;
case UINT8:
{
uint8_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(uint8_t))v->setFunc)(tmp);
}
break;
case INT8:
{
int8_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(int8_t))v->setFunc)(tmp);
}
break;
case UINT16:
{
uint16_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(uint16_t))v->setFunc)(tmp);
}
break;
case INT16:
{
int16_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(int16_t))v->setFunc)(tmp);
}
break;
case UINT32:
{
uint32_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(uint32_t))v->setFunc)(tmp);
}
break;
case INT32:
{
int32_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(int32_t))v->setFunc)(tmp);
}
break;
case UINT64:
{
uint64_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(uint64_t))v->setFunc)(tmp);
}
break;
case INT64:
{
int64_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(int64_t))v->setFunc)(tmp);
}
break;
case FLOAT32:
{
float32_t tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(float32_t))v->setFunc)(tmp);
}
break;
case FLOAT64:
{
double tmp;
memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
((void (*)(double))v->setFunc)(tmp);
}
break;
}
}
}
}
break;
case PC_MESSAGE_SET_STREAMED_VAR:
if(dataBytesReady >= 1)
{
int i;
comm_nVarsToStream = rxDataBytesBuffer[0];
if(dataBytesReady != 1 + 1 + comm_nVarsToStream)
return;
comm_streamId = rxDataBytesBuffer[1];
for(i=0; i<comm_nVarsToStream; i++)
comm_streamedVars[i] = &comm_syncVars[rxDataBytesBuffer[2+i]];
}
break;
default: // No data bytes for the other message types.
break;
}
}
}
void comm_monitorVar(const char name[], void *address, comm_VarType type,
uint8_t size, comm_VarAccess access)
{
// Adding a variable to the list is not allowed if it has been locked, or if
// the list is full.
if(!comm_varListLocked && comm_nSyncVars < N_SYNCVARS_MAX)
{
comm_SyncVar v;
// Trim the name if it is too long.
if(strlen(name) > SYNCVAR_NAME_SIZE - 1)
{
memcpy(v.name, name, SYNCVAR_NAME_SIZE - 1);
v.name[SYNCVAR_NAME_SIZE - 1] = '\0';
}
else
strcpy(v.name, name);
//
v.address = address;
v.type = type;
v.size = size;
v.access = access;
v.usesVarAddress = true;
// Add the SyncVar to the list.
comm_syncVars[comm_nSyncVars] = v;
comm_nSyncVars++;
}
}
void comm_monitorVarFunc(const char name[], comm_VarType type, uint8_t size,
void (*getFunc)(void), void (*setFunc)(void))
{
// Adding a variable to the list is not allowed if it has been locked.
if(!comm_varListLocked)
{
comm_SyncVar v;
// Trim the name if it is too long.
if(strlen(name) > SYNCVAR_NAME_SIZE - 1)
{
memcpy(&v.name, &name, SYNCVAR_NAME_SIZE - 1);
v.name[SYNCVAR_NAME_SIZE - 1] = '\0';
}
else
strcpy(v.name, name);
//
v.getFunc = getFunc;
v.setFunc = setFunc;
v.type = type;
v.size = size;
v.usesVarAddress = false;
// Determine the variable access.
if(getFunc != NULL && setFunc == NULL)
v.access = READONLY;
else if(getFunc == NULL && setFunc != NULL)
v.access = WRITEONLY;
else if(getFunc != NULL && setFunc != NULL)
v.access = READWRITE;
else
return; // No function provided at all, ignoring var.
// Add the SyncVar to the list.
comm_syncVars[comm_nSyncVars] = v;
comm_nSyncVars++;
}
}
void comm_monitorBool(const char name[], bool *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, BOOL, 1, access);
}
void comm_monitorUint8(const char name[], uint8_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, UINT8, 1, access);
}
void comm_monitorInt8(const char name[], int8_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, INT8, 1, access);
}
void comm_monitorUint16(const char name[], uint16_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, UINT16, 2, access);
}
void comm_monitorInt16(const char name[], int16_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, INT16, 2, access);
}
void comm_monitorUint32(const char name[], uint32_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, UINT32, 4, access);
}
void comm_monitorInt32(const char name[], int32_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, INT32, 4, access);
}
void comm_monitorUint64(const char name[], uint64_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, UINT64, 8, access);
}
void comm_monitorInt64(const char name[], int64_t *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, INT64, 8, access);
}
void comm_monitorFloat(const char name[], float *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, FLOAT32, 4, access);
}
void comm_monitorDouble(const char name[], double *address,
comm_VarAccess access)
{
comm_monitorVar(name, address, FLOAT64, 8, access);
}
void comm_monitorBoolFunc(const char name[],
bool (*getFunc)(void), void (*setFunc)(bool))
{
comm_monitorVarFunc(name, BOOL, 1, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorUint8Func(const char name[],
uint8_t (*getFunc)(void), void (*setFunc)(uint8_t))
{
comm_monitorVarFunc(name, UINT8, 1, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorInt8Func(const char name[],
int8_t (*getFunc)(void), void (*setFunc)(int8_t))
{
comm_monitorVarFunc(name, INT8, 1, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorUint16Func(const char name[],
uint16_t (*getFunc)(void),
void (*setFunc)(uint16_t))
{
comm_monitorVarFunc(name, UINT16, 2, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorInt16Func(const char name[],
int16_t (*getFunc)(void), void (*setFunc)(int16_t))
{
comm_monitorVarFunc(name, INT16, 2, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorUint32Func(const char name[],
uint32_t (*getFunc)(void),
void (*setFunc)(uint32_t))
{
comm_monitorVarFunc(name, UINT32, 4, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorInt32Func(const char name[],
int32_t (*getFunc)(void), void (*setFunc)(int32_t))
{
comm_monitorVarFunc(name, INT32, 4, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorUint64Func(const char name[],
uint64_t (*getFunc)(void),
void (*setFunc)(uint64_t))
{
comm_monitorVarFunc(name, UINT64, 8, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorInt64Func(const char name[],
int64_t (*getFunc)(void), void (*setFunc)(int64_t))
{
comm_monitorVarFunc(name, INT64, 8, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorFloatFunc(const char name[],
float (*getFunc)(void), void (*setFunc)(float))
{
comm_monitorVarFunc(name, FLOAT32, 4, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_monitorDoubleFunc(const char name[],
double (*getFunc)(void), void (*setFunc)(double))
{
comm_monitorVarFunc(name, FLOAT64, 8, (void (*)(void))getFunc, (void (*)(void))setFunc);
}
void comm_LockSyncVarsList(void)
{
comm_varListLocked = true;
}
void comm_SendVarsList(void)
{
int i;
uint8_t *p;
uint16_t buffLength = 1 + comm_nSyncVars * (SYNCVAR_NAME_SIZE + 1 + 1);
p = &txBuffer[0];
*p = comm_nSyncVars;
p++;
for(i=0; i<comm_nSyncVars; i++)
{
strcpy((char*)p, comm_syncVars[i].name);
p += SYNCVAR_NAME_SIZE;
*p = (uint8_t)comm_syncVars[i].type;
p++;
*p = (uint8_t)comm_syncVars[i].access;
p++;
}
comm_SendPacket(STM_MESSAGE_VARS_LIST, txBuffer, buffLength);
}
/**
* @brief Sends a debug message to the computer.
* @param format format string.
* @param ... variables to be printed in the format string.
*/
void comm_SendDebugMessage(const char* format, ...)
{
uint16_t length;
va_list args;
va_start(args, format);
length = vsnprintf(comm_debugMessageBuffer, DEBUG_MESSAGE_BUFFER_SIZE,
format, args);
comm_SendPacket(STM_MESSAGE_DEBUG_TEXT, (uint8_t*)comm_debugMessageBuffer,
length+1);
va_end(args);
}

Event Timeline

c4science · Help