예제 #1
0
/**
 * @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;
}
예제 #2
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;
}
예제 #3
0
파일: overosync.c 프로젝트: CheBuzz/TauLabs
/**
 * 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;
			}
		}
	}
}
예제 #4
0
/**
 * 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;
}
예제 #5
0
/**
 * 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;
}