u32 m2r_read(u8 *buff, u32 n, void* context)
{
(void)context;
if(n == 0)
return 0;
return sdRead(&M2R_PORT, buff, n);
}
DRESULT disk_read (
BYTE pdrv, /* Physical drive nmuber to identify the drive */
BYTE *buff, /* Data buffer to store read data */
DWORD sector, /* Start sector in LBA */
UINT count /* Number of sectors to read */
)
{
DRESULT res;
// int result;
// switch (pdrv) {
// case DEV_RAM :
// // translate the arguments here
// result = RAM_disk_read(buff, sector, count);
// // translate the reslut code here
// return res;
// case DEV_MMC :
// // translate the arguments here
// result = MMC_disk_read(buff, sector, count);
// // translate the reslut code here
// return res;
// case DEV_USB :
// // translate the arguments here
// result = USB_disk_read(buff, sector, count);
// // translate the reslut code here
// return res;
// }
// return RES_PARERR;
if (pdrv || !count)
{
return RES_PARERR;
}
res = sdRead(sector, buff, count);
if(res == 0x00)
{
return RES_OK;
}
else
{
return RES_ERROR;
}
}
static THD_FUNCTION(Receive, arg) {
(void) arg; // Unused parameter
while (1)
{
palClearPad(GPIOB, GPIOB_LED1);
char c;
sdRead(&SD1, &c, 1);
palSetPad(GPIOB, GPIOB_LED1);
sdPut(&SD2, c);
}
}
int _read_r(struct _reent *r, int file, char * ptr, int len)
{
(void)r;
#if defined(STDIN_SD)
if (!len || (file != 0)) {
__errno_r(r) = EINVAL;
return -1;
}
len = sdRead(&STDIN_SD, (uint8_t *)ptr, (size_t)len);
return len;
#else
(void)file;
(void)ptr;
(void)len;
__errno_r(r) = EINVAL;
return -1;
#endif
}
/**
* @brief The Main receiving function.
* @details The SDReceiving thread responsible for receiving frames
* continuously from the serial.
*/
static THD_FUNCTION(SDReceiving, arg) {
chRegSetThreadName("Main Receiving Func");
DLLDriver *driver = arg;
while(true)
{
int i;
for(i = 0; i < FRAME_SIZE_BYTE; i++)
sdRead(driver->config->SDriver, &driver->DLLTempBuffer[i], 1);
if(CheckCRC(&driver->DLLTempBuffer) == 0)
{
driver->DLLStats.ReceivedFrames++;
}else
{
chMtxLock(&driver->DLLSerialSendMutex);
DLLSyncProcedure(driver);
chMtxUnlock(&driver->DLLSerialSendMutex);
}
}
}
static THD_FUNCTION (rxListen, arg)
{
(void)(arg);
uint8_t buffer;
while(!0)
{
/* Read arg3 bytes from device pointed by arg1(SD3 for this example)
* and write data to arg2(buffer for this example). Type of buffer
* should be (uint8_t *). This function blocks this thread until
* desired number of bytes have read*/
//sdRead(&SD3, &header, 9);
sdRead(&SD3, &buffer, 1);
/* Post contents of arg2 to mailbox pointed by arg1, wait to post maximum arg3 mSeconds
* arg3 may be TIME_IMMEDIATE(post if you can) or TIME_INFINITE(wait until post).
* If mailbox object is full, function waits for a free slot for arg3 mSeconds */
chMBPost(&serialMbox, buffer, TIME_INFINITE);
chThdSleepMilliseconds(10);
}
}
int _read_r(struct _reent *r, int file, char *ptr, int len)
{
char i = 0;
uint8_t buffersize = len;
//char ascii;
(void)r;
#if defined(STDIN_SD)
if (!len || (file != 0)) {
__errno_r(r) = EINVAL;
return -1;
}
len = sdRead(&STDIN_SD, (uint8_t *)ptr, (size_t)len);
return len;
#else
//chprintf((BaseSequentialStream *)&itm_port, "file descript: %i\n", file);
if (!len || (file == -1)) {
__errno_r(r) = EBADF;
return -1;
}else {
do{
if(lcfg_table[file].data[i] != EOF){
*ptr = lcfg_table[file].data[i++];
//ascii = *ptr;
//chprintf((BaseSequentialStream *)&itm_port, "character: %c in file: %i with len: %i\n", ascii, file, len);
ptr++;
}
len--;
}while((len > 0) && (lcfg_table[file].data[i] != EOF));
return buffersize - len;
}
__errno_r(r) = EINVAL;
return -1;
#endif
}
bool read(void* ptr, uint32_t size)
{
return sdRead(mSD, (uint8_t*)ptr, size) == size;
}
int serialReceive( uint8_t * data, int maxSz )
{
int cnt = sdRead( &SERIAL_UART, data, maxSz );
return cnt;
}
/*
* Application entry point.
*/
int main(void) {
int8_t accelData[2]={0,0}; // Discovery Board's Accelerometer
uint8_t receivedBuff[4]={0,0,0,0}; // Received request/information from PandaBoard
uint8_t sentData[4] = {0,0,0,0}; // Returned Information (reply) to the PandaBoard
float imuData[7]={0,0,0,0,0,0,0}; // IMU calculated data based on Razor Boad
int* razorInfo; // Razor Board Data
int steering = 0;
int speed = 0;
int ir_data[3]={0,0,0};
int16_t us_data[3]={0,0,0};
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
mypwmInit();
// Initializing Motor
motorInit();
// Initializing IR Thread
ADCinit();
// Initializing US Thread
// myUltrasonicInit();
// Initializing Discovery Board's Accelerometer
//mySPIinit();
// Initializing Razor Board
myRazorInit();
// Activates the USB driver and then the USB bus pull-up on D+.
myUSBinit();
// Initializing IMU Calculations.
initIMU();
//Starting the usb configuration
sdStart(&SDU1,&portConfig2);
char receivedInfo[11];
/*
* Main loop, it takes care of reciving the requests from Panda Board using USB protocol,
* and reply with the requested data.
*/
while (TRUE) {
receivedInfo[0]='T';
sdRead(&SDU1, receivedInfo, 10);
// getImuValues(imuData);
// getAccel(accelData);
// getIR(ir_data);
// getUS(us_data);
if(receivedInfo[0] != 'T'){
receivedInfo[11]='\0';
parse(receivedInfo);
//setMotorData(-(rcvData[1]-28),rcvData[2]-2);
setMotorData(rcvData[1],1550);
translate(rcvData[0],ir_data,us_data,razorInfo,imuData,accelData,sentData);
sdWrite(&SDU1, sentData, 4);
}
}
}
