/**
* @brief Saves one object instance per sector
* @param[in] obj UAVObjHandle the object to save
* @param[in] instId The instance of the object to save
* @return 0 if success or -1 if failure
* @note This uses one sector on the flash chip per object so that no buffering in ram
* must be done when erasing the sector before a save
*/
int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data)
{
uint32_t objId = UAVObjGetID(obj);
uint8_t crc = 0;
int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId);
// Object currently not saved
if(addr < 0)
addr = PIOS_FLASHFS_GetNewAddress(objId, instId);
// Could not allocate a sector
if(addr < 0)
return -1;
struct fileHeader header = {
.id = objId,
.instId = instId,
.size = UAVObjGetNumBytes(obj)
};
if(PIOS_Flash_W25X_EraseSector(addr) != 0)
return -2;
// Save header
// This information IS redundant with the object table id. Oh well. Better safe than sorry.
if(PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0)
return -3;
// Update CRC
crc = PIOS_CRC_updateCRC(0, (uint8_t *) &header, sizeof(header));
// Save data
if(PIOS_Flash_W25X_WriteData(addr + sizeof(header), data, UAVObjGetNumBytes(obj)) != 0)
return -4;
// Update CRC
crc = PIOS_CRC_updateCRC(crc, (uint8_t *) data, UAVObjGetNumBytes(obj));
// Save CRC (written so will work when CRC changes to uint16)
if(PIOS_Flash_W25X_WriteData(addr + sizeof(header) + UAVObjGetNumBytes(obj), (uint8_t *) &crc, sizeof(crc)) != 0)
return -4;
return 0;
}
/**
* @brief Load one object instance per sector
* @param[in] obj UAVObjHandle the object to save
* @param[in] instId The instance of the object to save
* @return 0 if success or error code
* @retval -1 if object not in file table
* @retval -2 if unable to retrieve object header
* @retval -3 if loaded data instId or objId don't match
* @retval -4 if unable to retrieve instance data
* @retval -5 if unable to read CRC
* @retval -6 if CRC doesn't match
* @note This uses one sector on the flash chip per object so that no buffering in ram
* must be done when erasing the sector before a save
*/
int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data)
{
uint32_t objId = UAVObjGetID(obj);
uint16_t objSize = UAVObjGetNumBytes(obj);
uint8_t crc = 0;
uint8_t crcFlash = 0;
const uint8_t crc_read_step = 8;
uint8_t crc_read_buffer[crc_read_step];
int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId);
// Object currently not saved
if(addr < 0)
return -1;
struct fileHeader header;
// Load header
// This information IS redundant with the object table id. Oh well. Better safe than sorry.
if(PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)) != 0)
return -2;
// Update CRC
crc = PIOS_CRC_updateCRC(0, (uint8_t *) &header, sizeof(header));
if((header.id != objId) || (header.instId != instId))
return -3;
// To avoid having to allocate the RAM for a copy of the object, we read by chunks
// and compute the CRC
for(uint32_t i = 0; i < objSize; i += crc_read_step) {
PIOS_Flash_W25X_ReadData(addr + sizeof(header) + i, crc_read_buffer, crc_read_step);
uint8_t valid_bytes = ((i + crc_read_step) >= objSize) ? objSize - i : crc_read_step;
crc = PIOS_CRC_updateCRC(crc, crc_read_buffer, valid_bytes);
}
// Read CRC (written so will work when CRC changes to uint16)
if(PIOS_Flash_W25X_ReadData(addr + sizeof(header) + objSize, (uint8_t *) &crcFlash, sizeof(crcFlash)) != 0)
return -5;
if(crc != crcFlash)
return -6;
// Read the instance data
if (PIOS_Flash_W25X_ReadData(addr + sizeof(header), data, objSize) != 0)
return -4;
return 0;
}
/**
* Telemetry transmit task, regular priority
*
* Logic: We need to double buffer the DMA transfers. Pack the buffer until either
* 1) it is full (and then we should record the number of missed events then)
* 2) the current transaction is done (we should immediately schedule since we are slave)
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
* and then take the semaphrore
*/
static void overoSyncTask(void *parameters)
{
UAVObjEvent ev;
// Kick off SPI transfers (once one is completed another will automatically transmit)
overosync->transaction_done = true;
overosync->sent_objects = 0;
overosync->failed_objects = 0;
overosync->received_objects = 0;
uint32_t lastUpdateTime = PIOS_Thread_Systime();
uint32_t updateTime;
fid = fopen("sim_log.opl", "w");
// Loop forever
while (1) {
// Wait for queue message
if (PIOS_Queue_Receive(queue, &ev, PIOS_QUEUE_TIMEOUT_MAX) == true) {
// Check it will fit before packetizing
if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >=
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
overosync->failed_objects ++;
} else {
// Process event. This calls transmitData
UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
}
updateTime = PIOS_Thread_Systime();
if(((uint32_t) (updateTime - lastUpdateTime)) > 1000) {
// Update stats. This will trigger a local send event too
OveroSyncStatsData syncStats;
syncStats.Send = overosync->sent_bytes;
syncStats.Received = 0;
syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE;
syncStats.DroppedUpdates = overosync->failed_objects;
OveroSyncStatsSet(&syncStats);
overosync->failed_objects = 0;
overosync->sent_bytes = 0;
lastUpdateTime = updateTime;
}
}
}
}
/**
* Process an byte from the telemetry stream.
* \param[in] connection UAVLinkConnection to be used
* \param[in] rxbyte Received byte
* \return UAVLinkRxState
*/
UAVLinkRxState UAVLinkProcessInputStreamQuiet(UAVLinkConnection connectionHandle, uint8_t rxbyte)
{
UAVLinkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
UAVLinkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVLINK_STATE_ERROR || iproc->state == UAVLINK_STATE_COMPLETE || iproc->state == UAVLINK_STATE_STREAM_COMPLETE)
iproc->state = UAVLINK_STATE_SYNC;
if (iproc->rxPacketLength < 0xffff)
iproc->rxPacketLength++; // update packet byte count
// Receive state machine
switch (iproc->state)
{
case UAVLINK_STATE_SYNC:
if (rxbyte != UAVLINK_SYNC_VAL)
break;
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->state = UAVLINK_STATE_TYPE;
break;
case UAVLINK_STATE_TYPE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if ((rxbyte & UAVLINK_TYPE_MASK) != UAVLINK_TYPE_VER)
{
iproc->state = UAVLINK_STATE_ERROR;
break;
}
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVLINK_STATE_SIZE;
iproc->rxCount = 0;
break;
case UAVLINK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0)
{
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
if (iproc->packet_size < UAVLINK_MIN_PACKET_SIZE || iproc->packet_size > UAVLINK_MAX_PACKET_LENGTH)
{ // incorrect packet size
iproc->state = UAVLINK_STATE_ERROR;
break;
}
iproc->rxCount = 0;
iproc->rxId = 0;
if (iproc->type == UAVLINK_TYPE_STREAM)
{
iproc->state = UAVLINK_STATE_STREAMID;
} else {
iproc->state = UAVLINK_STATE_OBJID;
}
break;
case UAVLINK_STATE_STREAMID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->rxId = rxbyte;
iproc->length = iproc->packet_size - iproc->rxPacketLength;
iproc->state = UAVLINK_STATE_DATA;
break;
case UAVLINK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->rxId += rxbyte << (8*(iproc->rxCount++));
if (iproc->rxCount < 4)
break;
// Search for object.
iproc->obj = UAVObjGetByID(iproc->rxId);
// Determine data length
if (iproc->type == UAVLINK_TYPE_OBJ_REQ || iproc->type == UAVLINK_TYPE_ACK || iproc->type == UAVLINK_TYPE_NACK)
{
iproc->length = 0;
iproc->instanceLength = 0;
}
else
{
if (iproc->obj)
{
iproc->length = UAVObjGetNumBytes(iproc->obj);
iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2);
}
else
{
// We don't know if it's a multi-instance object, so just assume it's 0.
iproc->instanceLength = 0;
iproc->length = iproc->packet_size - iproc->rxPacketLength;
}
}
// Check length and determine next state
if (iproc->length >= UAVLINK_MAX_PAYLOAD_LENGTH)
{
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->instanceLength + iproc->length) != iproc->packet_size)
{ // packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
}
iproc->instId = 0;
if (iproc->type == UAVLINK_TYPE_NACK)
{
// If this is a NACK, we skip to Checksum
iproc->state = UAVLINK_STATE_CS;
}
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj))
{
iproc->state = UAVLINK_STATE_INSTID;
}
else
{
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0)
iproc->state = UAVLINK_STATE_DATA;
else
iproc->state = UAVLINK_STATE_CS;
}
iproc->rxCount = 0;
break;
case UAVLINK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8*(iproc->rxCount++));
if (iproc->rxCount < 2)
break;
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0)
iproc->state = UAVLINK_STATE_DATA;
else
iproc->state = UAVLINK_STATE_CS;
break;
case UAVLINK_STATE_DATA:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length)
break;
iproc->state = UAVLINK_STATE_CS;
iproc->rxCount = 0;
break;
case UAVLINK_STATE_CS:
// the CRC byte
if (rxbyte != iproc->cs)
{ // packet error - faulty CRC
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
}
if (iproc->type == UAVLINK_TYPE_STREAM)
{
connection->stats.rxStreamBytes += iproc->length;
connection->stats.rxStreamPackets++;
iproc->state = UAVLINK_STATE_COMPLETE;
} else {
if (iproc->rxPacketLength != (iproc->packet_size + 1))
{ // packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
} else {
connection->stats.rxObjectBytes += iproc->length;
connection->stats.rxObjects++;
iproc->state = UAVLINK_STATE_COMPLETE;
}
}
break;
default:
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
}
// Done
return iproc->state;
}
/**
* Process an byte from the telemetry stream.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \return UAVTalkRxState
*/
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
iproc->state = UAVTALK_STATE_SYNC;
}
if (iproc->rxPacketLength < 0xffff) {
// update packet byte count
iproc->rxPacketLength++;
}
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
break;
}
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
case UAVTALK_STATE_TYPE:
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
break;
case UAVTALK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0) {
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case UAVTALK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->timestampLength = 0;
} else {
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
}
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
}
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
break;
case UAVTALK_STATE_DATA:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length) {
break;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
break;
case UAVTALK_STATE_CS:
// Check the CRC byte
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
default:
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Done
return iproc->state;
}