/*!
* @brief Run on the main thread to handle new accelerometer data.
*/
void HandleNewAccelData()
{
if (Accel_GetMode() == ACCEL_INT)
{
Packet_Put(0x10, AccReadData[0], AccReadData[1], AccReadData[2]);
return;
}
//Shift history
ShiftArray(AccelXHistory, 3, AccReadData[0]);
ShiftArray(AccelYHistory, 3, AccReadData[1]);
ShiftArray(AccelZHistory, 3, AccReadData[2]);
uint8_t xMed = Median_Filter3(AccelXHistory[0], AccelXHistory[1], AccelXHistory[2]);
uint8_t yMed = Median_Filter3(AccelYHistory[0], AccelYHistory[1], AccelYHistory[2]);
uint8_t zMed = Median_Filter3(AccelZHistory[0], AccelZHistory[1], AccelZHistory[2]);
if ((xMed != AccelSendHistory[0]) | (yMed != AccelSendHistory[1]) | (zMed != AccelSendHistory[2]))
{
AccelSendHistory[0] = xMed;
AccelSendHistory[1] = yMed;
AccelSendHistory[2] = zMed;
(void) CMD_SendAccelerometerValues(AccelSendHistory);
}
}
/**
* \fn BOOL HandleModConUptime(void)
* \brief Sends out system uptime in minutes and seconds
* \return TRUE if the packet has queued successfully.
*/
BOOL HandleModConUptime(void)
{
if (!Packet_Put(MODCON_COMMAND_TIME, MODCON_TIME_INITIAL, Clock_Seconds, Clock_Minutes))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
/**
* \fn BOOL HandleModConProtocolModeGet(void)
* \brief Builds a packet that contains ModCon protocol mode and places it into transmit buffer.
* \return TRUE if the packet was queued for transmission successfully.
*/
BOOL HandleModConProtocolModeGet(void)
{
if (!Packet_Put(MODCON_COMMAND_PROTOCOL_MODE, MODCON_PROTOCOL_MODE_GET, (UINT8)ModConProtocolMode ,0))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
/**
* \fn BOOL HandleModConSpecialVersion(void)
* \brief Builds a packet that contains ModCon version details and places it into transmit buffer.
* \return TRUE if the packet was queued for transmission successfully.
*/
BOOL HandleModConSpecialVersion(void)
{
if (!Packet_Put(MODCON_COMMAND_SPECIAL, MODCON_VERSION_INITIAL, MODCON_VERSION_MAJOR, MODCON_VERSION_MINOR))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
/**
* \fn BOOL HandleModConModeGet(void)
* \brief Builds a packet that contains ModCon mode and places it into transmit buffer.
* \return TRUE if the packet was queued for transmission successfully.
*/
BOOL HandleModConModeGet(void)
{
TUINT16 mode;
mode.l = ModConMode;
if (!Packet_Put(MODCON_COMMAND_MODE, MODCON_MODE_GET, mode.s.Lo, mode.s.Hi))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
/**
* \fn BOOL HandleModConNumberGet(void)
* \brief Builds a packet that contains ModCon number and places it into transmit buffer.
* \return TRUE if the packet was queued for transmission successfully.
*/
BOOL HandleModConNumberGet(void)
{
TUINT16 number;
number.l = ModConNumber;
if (!Packet_Put(MODCON_COMMAND_NUMBER, MODCON_NUMBER_GET, number.s.Lo, number.s.Hi))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
/*!
* @brief Handle incoming packets
*/
void HandlePacket()
{
BOOL error = bTRUE;
uint8_t data;
//mask out the ack, otherwise it goes to default
switch (Packet_Command & ~PACKET_ACK_MASK)
{
case CMD_RX_SPECIAL_GET_STARTUP_VALUES:
error = !CMD_SpecialGetStartupValues();
break;
case CMD_RX_FLASH_PROGRAM_BYTE:
error = !CMD_FlashProgramByte(Packet_Parameter1, Packet_Parameter3);
break;
case CMD_RX_FLASH_READ_BYTE:
error = !CMD_FlashReadByte(Packet_Parameter1);
break;
case CMD_RX_SPECIAL_GET_VERSION:
error = !CMD_SpecialTowerVersion();
break;
case CMD_RX_TOWER_NUMBER:
error = !CMD_TowerNumber(Packet_Parameter1, Packet_Parameter2, Packet_Parameter3);
break;
case CMD_RX_TOWER_MODE:
error = !CMD_TowerMode(Packet_Parameter1, Packet_Parameter2, Packet_Parameter3);
break;
case CMD_RX_SET_TIME:
error = !CMD_SetTime(Packet_Parameter1, Packet_Parameter2, Packet_Parameter3);
break;
case CMD_RX_PROTOCOL_MODE:
error = !CMD_ProtocolMode(Packet_Parameter1, Packet_Parameter2, Packet_Parameter3);
break;
default:
break;
}
if (Packet_Command & PACKET_ACK_MASK)
{
uint8_t maskedPacket = 0;
if (error)
{
maskedPacket = Packet_Command & ~PACKET_ACK_MASK;
}
else
{
maskedPacket = Packet_Command | PACKET_ACK_MASK;
}
Packet_Put(maskedPacket, Packet_Parameter1, Packet_Parameter2, Packet_Parameter3);
}
}
/**
* \fn BOOL HandleModConAnalogInputValue(const TAnalogChannel channelNb)
* \brief Builds a packet that contains current ModCon analog input value of selected channel and places it into transmit buffer.
* \return TRUE if the command has been executed successfully.
*/
BOOL HandleModConAnalogInputValue(const TAnalogChannel channelNb)
{
UINT8 index = 0xFF;
/* NOTE: channel enums might not be in numberic order */
switch(channelNb)
{
case ANALOG_INPUT_Ch1:
index = 0;
break;
case ANALOG_INPUT_Ch2:
index = 1;
break;
case ANALOG_INPUT_Ch3:
index = 2;
break;
case ANALOG_INPUT_Ch4:
index = 3;
break;
case ANALOG_INPUT_Ch5:
index = 4;
break;
case ANALOG_INPUT_Ch6:
index = 5;
break;
case ANALOG_INPUT_Ch7:
index = 6;
break;
case ANALOG_INPUT_Ch8:
index = 7;
break;
default:
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_INVALID_ARGUMENT);
#endif
return bFALSE;
break;
}
if (!Packet_Put(MODCON_COMMAND_ANALOG_INPUT_VALUE, index, Analog_Input[index].Value.s.Lo, Analog_Input[index].Value.s.Hi))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
/**
* \fn BOOL HandleModConEEPROMGet(void)
* \brief Return byte value of given EEPROM address.
* \return TRUE if address is validated and the packet was queued for transmission successfully.
*/
BOOL HandleModConEEPROMGet(void)
{
UINT8 volatile * const address = (UINT8 volatile *)Packet_Parameter12;
if (!Packet_Parameter3 && EEPROM_ValidateAddress((void * const)address))
{
if (!Packet_Put(MODCON_COMMAND_EEPROM_GET, Packet_Parameter1, Packet_Parameter2, *address))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return bTRUE;
}
return bFALSE;
}
/**
* \fn BOOL HandleModConStartup(void)
* \brief Builds packets that are necessary for startup information and places them into transmit buffer.
* \return TRUE if packets were queued for transmission successfully.
*/
BOOL HandleModConStartup(void)
{
/* it should contain zeros */
if (!Packet_Parameter1 && !Packet_Parameter23)
{
/* push packets of ModCon startup figures */
if (!Packet_Put(MODCON_COMMAND_STARTUP, 0, 0, 0))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
return bFALSE;
}
return HandleModConSpecialVersion() &&
HandleModConProtocolModeGet() &&
HandleModConNumberGet() &&
HandleModConModeGet();
}
return bFALSE;
}
/**
* \fn void Routine(void)
* \brief Retrieves ModCon packets and sends back packets if it is necessary.
*/
void Routine(void)
{
UINT8 ack = 0;
BOOL bad = bFALSE;
if (Clock_Update())
{
bad = !HandleModConUptime();
}
if (Packet_Get())
{
ack = Packet_Command & MODCON_COMMAND_ACK_MASK; /* detect ACK mask from command */
Packet_Command &= ~MODCON_COMMAND_ACK_MASK; /* clear ACK mask from command */
switch(Packet_Command)
{
case MODCON_COMMAND_STARTUP:
bad = !HandleModConStartup();
break;
case MODCON_COMMNAD_EEPROM_PROGRAM:
bad = !HandleModConEEPROMProgram();
break;
case MODCON_COMMAND_EEPROM_GET:
bad = !HandleModConEEPROMGet();
break;
case MODCON_COMMAND_SPECIAL:
bad = !HandleModConSpecial();
break;
case MODCON_COMMAND_PROTOCOL_MODE:
bad = !HandleModConProtocolMode();
break;
case MODCON_COMMAND_NUMBER:
bad = !HandleModConNumber();
break;
case MODCON_COMMAND_MODE:
bad = !HandleModConMode();
break;
default:
bad = bTRUE;
break;
}
if (ack)
{
if (!bad)
{
if (!Packet_Put(Packet_Command | MODCON_COMMAND_ACK_MASK, Packet_Parameter1, Packet_Parameter2, Packet_Parameter3))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
}
}
else
{ /* NOTE: ACK mask has been cleared already */
if (!Packet_Put(Packet_Command, Packet_Parameter1, Packet_Parameter2, Packet_Parameter3))
{
#ifndef NO_DEBUG
DEBUG(__LINE__, ERR_PACKET_PUT);
#endif
}
}
}
}
}
