Homec4science
Diffusion HHRI-software (hri-gen2) Browse

R2671/Firmware/src/communication.c
4b712c87ee2chri-gen2

Last Change
Raw File

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"
extern uint32_t statusReg, ctrl_timestamp, ctrl_currentFilterType;
extern float32_t ctrl_motorPosCod, ctrl_motorPosCod_c;
extern float32_t ctrl_motorTorque_Nm_c, ctrl_motorPosCodFilt, ctrl_firstOrderFilterTau;
extern float32_t hall_angle, tac_speed, sg_force;
extern pid_Pid ctrl_positionPid;
extern bfilt_BasicFilter ctrl_encoderFilter;
extern float32_t ctrl_sineAmplitude, ctrl_sinePeriod;
uint32_t selectedVariablesToStream; // Bitfield that indicates for each variable if it should be streamed or not.
void* variablesArray[N_VARIABLES]; // Pointers to variables to stream.
uint8_t txBuffer[1024];
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).
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);
/**
* @brief Init the communication manager.
*/
void comm_Init(void)
{
// Initialize the array with all the pointers on variables.
// Note that this array contains pointers only to uint32_t, even if the
// variables are actually float32_t. This does not matter, since we only use
// these pointers to perform byte-to-byte copies.
// Do not modify this by hand, copy-paste code generated by the excel sheet.
variablesArray[VAR_TIMESTAMP] = (uint32_t*) &ctrl_timestamp;
variablesArray[VAR_POS_REG_PID_P] = (uint32_t*) &ctrl_positionPid.kp;
variablesArray[VAR_POS_REG_PID_I] = (uint32_t*) &ctrl_positionPid.ki;
variablesArray[VAR_POS_REG_PID_D] = (uint32_t*) &ctrl_positionPid.kd;
variablesArray[VAR_POS_REG_PID_ARW] = (uint32_t*) &ctrl_positionPid.arw;
variablesArray[VAR_POS_REG_PID_INTEGRATOR] = (uint32_t*) &ctrl_positionPid.integrator;
variablesArray[VAR_POS_REG_PID_OUTPUT] = (uint32_t*) &ctrl_motorTorque_Nm_c;
variablesArray[VAR_POS_REG_SAMPLING_PERIOD] = NULL;
variablesArray[VAR_WALL_POSITION] = (uint32_t*) &ctrl_motorPosCod_c;
variablesArray[VAR_MEASURED_ANGLE_RAW] = NULL;
variablesArray[VAR_MEASURED_ANGLE_FILTERED] = (uint32_t*) &ctrl_motorPosCodFilt;
variablesArray[VAR_COMM_TX_PERIOD] = NULL;
variablesArray[VAR_ACTIVE_FILTER] = (uint32_t*) &ctrl_currentFilterType;
variablesArray[VAR_1ST_FILT_TAU] = (uint32_t*) &ctrl_encoderFilter.tau;
variablesArray[VAR_DAC1_VOLTAGE] = NULL;
variablesArray[VAR_DAC2_VOLTAGE] = NULL;
variablesArray[VAR_ADC1_VOLTAGE] = NULL;
variablesArray[VAR_ADC2_VOLTAGE] = NULL;
variablesArray[VAR_HALL_ANGLE] = NULL;
variablesArray[VAR_TACHO_SPEED] = NULL;
variablesArray[VAR_STRAIN_GAUGE_FORCE] = NULL;
variablesArray[VAR_USER_VAR_1] = NULL;
variablesArray[VAR_USER_VAR_2] = NULL;
variablesArray[VAR_USER_VAR_3] = NULL;
variablesArray[VAR_USER_VAR_4] = NULL;
variablesArray[VAR_USER_VAR_5] = NULL;
// Setup the UART peripheral, and specify the function that will be called
// each time a byte is received.
uart_Init(comm_HandleByte);
// Make the streaming function periodically called by the timer 7.
cbt_SetCommLoopTimer(comm_Stream, TE_DATA_LOOP_DEFAULT_VAL);
}
/**
* @brief UART data streaming "loop" function.
*/
void comm_Stream()
{
// If the data streaming is enabled, send a stream packet to the PC.
if(statusReg & DATA_UPSTREAM_ENB)
{
uint32_t i;
int nDataBytesToSend = 0;
for(i=0; i<N_VARIABLES; i++)
{
if(selectedVariablesToStream & (1<<i)) // If the variable has been selected for streaming...
{
uint32_t variableValue;
variableValue = comm_GetVar(i);
memcpy(&txBuffer[nDataBytesToSend], &variableValue, 4);
nDataBytesToSend += 4;
}
}
comm_SendPacket(STM_MESSAGE_STREAMING_PACKET, txBuffer, nDataBytesToSend);
}
}
/**
* @brief Get the four bytes of a monitored variable, from its index.
* @param variableIndex: index of the variable to get.
* @return the variable bytes as a uint32_t (even if actually float32_t).
* @note This function returns 0 if the variableIndex does not correspond to a
* valid variable to read.
*/
uint32_t comm_GetVar(uint8_t variableIndex)
{
float32_t tmp;
uint32_t variableValue;
// Note that some variable cannot be read directly, and are
// treated as exceptions, because a function call is necessary.
switch(variableIndex)
{
// Special case, function call needed.
case VAR_POS_REG_SAMPLING_PERIOD:
variableValue = cbt_GetPositionLoopPeriod();
break;
case VAR_COMM_TX_PERIOD:
variableValue = cbt_GetCommLoopPeriod();
break;
case VAR_DAC1_VOLTAGE:
tmp = dac_GetVoltage1();
variableValue = *(uint32_t*)&tmp;
break;
case VAR_DAC2_VOLTAGE:
tmp = dac_GetVoltage2();
variableValue = *(uint32_t*)&tmp;
break;
case VAR_ADC1_VOLTAGE:
tmp = adc_GetChannelVoltage(ANIN1, ADC_SCALE_10_V);
variableValue = *(uint32_t*)&tmp;
break;
case VAR_ADC2_VOLTAGE:
tmp = adc_GetChannelVoltage(ANIN2, ADC_SCALE_10_V);
variableValue = *(uint32_t*)&tmp;
break;
case VAR_MEASURED_ANGLE_RAW:
tmp = enc_GetPosition();
variableValue = *(uint32_t*)&tmp;
break;
// All the others are normal 32-bits variables.
default:
if((variableIndex < N_VARIABLES) && (variablesArray[variableIndex] != NULL)) // Check if the index is OK.
variableValue = *((uint32_t*)variablesArray[variableIndex]);
else
variableValue = 0;
break;
}
return variableValue;
}
/**
* @brief Set the four bytes of a monitored variable, from its index.
* @param variableIndex: index of the variable to get.
* @param newValue: new value for the variable.
* @note This function does nothing if the variableIndex does not correspond to
* a valid variable to write.
*/
void comm_SetVar(uint8_t variableIndex, uint32_t newValue)
{
// Copy the new value to the selected 32-bits variable.
// Note that some variable cannot be changed directly, and
// are treated as exceptions, because a function call is necessary.
switch(variableIndex)
{
// Special case, function call needed.
case VAR_POS_REG_SAMPLING_PERIOD:
cbt_SetPositionLoopPeriod(newValue);
break;
case VAR_COMM_TX_PERIOD:
cbt_SetCommLoopPeriod(newValue);
break;
case VAR_DAC1_VOLTAGE:
dac_SetVoltage1(*(float32_t*)&newValue);
break;
case VAR_DAC2_VOLTAGE:
dac_SetVoltage2(*(float32_t*)&newValue);
break;
case VAR_MEASURED_ANGLE_RAW:
enc_SetPosition(*(float32_t*)&newValue);
break;
// All the others are normal 32-bits variables.
default:
if((variableIndex < N_VARIABLES) && (variablesArray[variableIndex] != NULL)) // Check if the index is OK.
memcpy(variablesArray[variableIndex], &newValue, 4);
break;
}
}
/**
* @brief Send Packet with a header and data bytes.
* @param messageType: message type (see the protocol doc).
* @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 messageType, uint8_t *data, uint16_t dataLength)
{
int i=0;
uart_SendByte((1<<7) | messageType);
for(i=0; i<dataLength; i++)
{
// Every data byte is splitted into two transmitted bytes.
uart_SendByte((data[i]&0xf0) >> 4); // Most significant bits.
uart_SendByte((data[i]&0x0f)); // Least significant bits.
}
}
/**
* @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_SEND_STATUS:
if(dataBytesReady == 0)
comm_SendPacket(STM_MESSAGE_STATUS, (uint8_t*)(&statusReg), 4);
break;
case PC_MESSAGE_START_STREAMING:
if(dataBytesReady == 0)
{
statusReg |= DATA_UPSTREAM_ENB;
TIM_SetCounter(TIM7, 0);
TIM_Cmd(TIM7, ENABLE);
}
break;
case PC_MESSAGE_STOP_STREAMING:
if(dataBytesReady == 0)
{
TIM_Cmd(TIM7, DISABLE);
statusReg &= ~DATA_UPSTREAM_ENB;
}
break;
case PC_MESSAGE_GET_VAR:
if(dataBytesReady == 1)
{
uint32_t variableValue;
// 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 >= N_VARIABLES)
return;
// Prepare the message to be sent to the PC.
// First byte: variable index.
txBuffer[0] = variableIndex;
// Four following bytes: actual value of the variable.
variableValue = comm_GetVar(variableIndex);
memcpy(&txBuffer[1], &variableValue, 4);
// Send the message to the PC.
comm_SendPacket(STM_MESSAGE_VAR, txBuffer, 5);
}
break;
case PC_MESSAGE_SET_VAR:
if(dataBytesReady == 5)
{
// Extract the index (that indicates which variable will change),
// and the new value of the variable.
uint32_t newValue;
uint8_t variableIndex = rxDataBytesBuffer[0];
memcpy(&newValue, &rxDataBytesBuffer[1], 4);
// Set the selected variable with the new value.
comm_SetVar(variableIndex, newValue);
}
break;
case PC_MESSAGE_SET_STREAMED_VAR:
if(dataBytesReady == 4)
{
uint8_t *newSelectedVars = &rxDataBytesBuffer[0];
memcpy(&selectedVariablesToStream, newSelectedVars, 4);
}
break;
default: // No data bytes for the other message types.
break;
}
}
}
c4science · Help