void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
if (link == DOWN_LINK) {
sdWrite(&SD1, data, size);
}
else {
sdWrite(&SD2, data, size);
}
}
/*
* Application entry point.
*/
int main(void) {
uint32 blinker_id;
/* HAL initialization, this also initializes the configured device drivers
and performs the board-specific initializations.*/
halInit();
/* OS initialization.*/
(void) OS_API_Init();
/* Activates the serial driver 2 using the driver default configuration.
PA2(TX) and PA3(RX) are routed to USART2.*/
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
/* Starting the blinker thread.*/
(void) OS_TaskCreate(&blinker_id, "blinker", blinker,
(uint32 *)wa_blinker, sizeof wa_blinker,
128, 0);
/* In the ChibiOS/RT OSAL implementation the main() function is an
usable thread with priority 128 (NORMALPRIO), here we just sleep
in a loop printing a message on the serial port.*/
while (true) {
sdWrite(&SD2, (uint8_t *)"Hello World!\r\n", 14);
OS_TaskDelay(500);
}
}
u32 m2r_write(u8 *buff, u32 n, void* context)
{
(void)context;
if(n == 0)
return 0;
return sdWrite(&M2R_PORT, buff, n);
}
/*
* Application entry point.
*/
int main(void) {
/*
* 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();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity.
*/
while (TRUE) {
chThdSleepMilliseconds(500);
if (!palReadPad(IOPORT2, PIOB_SW1))
sdWrite(&SD1, (uint8_t *)"Hello World!\r\n", 14);
if (!palReadPad(IOPORT2, PIOB_SW2))
TestThread(&SD1);
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
/* HAL initialization, this also initializes the configured device drivers
and performs the board-specific initializations.*/
halInit();
/* The kernel is initialized but not started yet, this means that
main() is executing with absolute priority but interrupts are
already enabled.*/
osKernelInitialize();
/* Activates the serial driver 2 using the driver default configuration.
PA2(TX) and PA3(RX) are routed to USART2.*/
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
/* Creates the example thread, it does not start immediately.*/
osThreadCreate(osThread(Thread1), NULL);
/* Kernel started, the main() thread has priority osPriorityNormal
by default.*/
osKernelStart();
/* In the ChibiOS/RT CMSIS RTOS implementation the main() is an
usable thread, here we just sleep in a loop printing a message.*/
while (true) {
sdWrite(&SD2, (uint8_t *)"Hello World!\r\n", 14);
osDelay(500);
}
}
static THD_FUNCTION(txWrite, arg)
{
(void)(arg);
while(!0){
chSysLock();
tp_W = chThdGetSelfX();
chSchGoSleepS(CH_STATE_SUSPENDED);
chSysUnlock();
sdWrite(&SD3, &temda, strlen(temda));
/* Get contents of next mail in mailbox pointed by arg1 to arg2
* and wait mail arg3 mSeconds if there is no message is present.
*
* Returns MSG_OK if succesfully a mail fetched from mailbox*/
//if(chMBFetch(&serialMbox, (msg_t *)&toSend, TIME_INFINITE) == MSG_OK)
// {
/* Write arg3 bytes from arg2 to device pointed by arg1(SD3 for this example).
* Type of arg2 should be (uint8_t *) otherwise only first 8 bit will be send.*/
// sdWrite(&SD3, &toSend,1);
//}
//wifiInit();
chThdSleepMilliseconds(1000);
}
}
int _write_r(struct _reent *r, int file, char * ptr, int len)
{
int n;
(void)r;
//(void)file;
(void)ptr;
#if defined(STDOUT_SD)
if (file != 1) {
__errno_r(r) = EINVAL;
return -1;
}
sdWrite(&STDOUT_SD, (uint8_t *)ptr, (size_t)len);
#endif
switch (file) {
case 1: /*stdout*/
case 2: /* stderr */
for (n = 0; n < len; n++) {
chprintf((BaseSequentialStream *)&itm_port, "%c", *ptr++);
}
break;
default:
errno = EBADF;
return -1;
}
return len;
}
/**
* @brief Send a 'FrameStruct' type pointer via serial
* @details If a sync happens the serial sending blocked by a mutex variable
* and the frame which need to be sent will lost.
*
* @param[in] driver DataLinkLayer driver structure
* @param[in] frame The frame which need to be sent
*
*/
bool DLLSendSingleFrameSerial(DLLDriver *driver, FrameStruct *Frame){
bool IsLocked = chMtxTryLock(&driver->DLLSerialSendMutex);
if(IsLocked){
sdWrite(driver->config->SDriver, Frame, FRAME_SIZE_BYTE);
palTogglePad(GPIOB, GPIOB_LED1);
chMtxUnlock(&driver->DLLSerialSendMutex);
}
return IsLocked;
}
DRESULT disk_write (
BYTE pdrv, /* Physical drive nmuber to identify the drive */
const BYTE *buff, /* Data to be written */
DWORD sector, /* Start sector in LBA */
UINT count /* Number of sectors to write */
)
{
DRESULT res;
// int result;
// switch (pdrv) {
// case DEV_RAM :
// // translate the arguments here
// result = RAM_disk_write(buff, sector, count);
// // translate the reslut code here
// return res;
// case DEV_MMC :
// // translate the arguments here
// result = MMC_disk_write(buff, sector, count);
// // translate the reslut code here
// return res;
// case DEV_USB :
// // translate the arguments here
// result = USB_disk_write(buff, sector, count);
// // translate the reslut code here
// return res;
// }
//return RES_PARERR;
if (pdrv || !count)
{
return RES_PARERR;
}
res = sdWrite(sector, buff, count);
if(res == 0)
{
return RES_OK;
}
else
{
return RES_ERROR;
}
}
void gps_write_cmd(uint8_t * cmd_buf, uint8_t len) {
uint8_t chksum = 0, i, num_len = 0;
char num_buf[4];
for (i = 0; i < len; i++)
chksum ^= cmd_buf[i];
chIOPut(&GPS_SERIAL, '$');
sdWrite(&GPS_SERIAL, cmd_buf, len);
chIOPut(&GPS_SERIAL, '*');
stoh(chksum, num_buf, &num_len);
sdWrite(&GPS_SERIAL, num_buf, num_len - 1);
chIOPut(&GPS_SERIAL, '\r');
chIOPut(&GPS_SERIAL, '\n');
}
static THD_FUNCTION(thEDisplay, arg)
{
(void)arg;
chRegSetThreadName("eDisplay");
kbb_display.header.preamble = KBB_PREAMBLE;
kbb_display.header.msg_class = KBB_CLASS_EXTERNAL;
kbb_display.header.msg_subclass = KBB_SUBCLASS_EXTERNAL_01;
kbb_display.header.msg_size = KBB_DISPLAY_SIZE;
while( !chThdShouldTerminateX() )
{
chSysLock();
eDisplayThreadForSleep = chThdGetSelfX();
chSchGoSleepS(CH_STATE_SUSPENDED);
chSysUnlock();
if ( !ed_serialPort )
continue;
if ( ed_serialPort == &Serial1 )
if ( kbse_getBaudSerial1() < 115200 )
continue;
if ( ed_serialPort == &Serial2 )
if ( kbse_getBaudSerial2() < 115200 )
continue;
const kbb_current_msg_t * msg = kbw_getCurrentMsg();
memcpy(&kbb_display.ltc_frame, &msg->ltc_frame, sizeof(kbb_display.ltc_frame));
memcpy(&kbb_display.smpte_time, &msg->smpte_time, sizeof(kbb_display.smpte_time));
kbb_display.ecef[0] = msg->nav_sol.ecefX;
kbb_display.ecef[1] = msg->nav_sol.ecefY;
kbb_display.ecef[2] = msg->nav_sol.ecefZ;
memcpy(&kbb_display.vnav, &msg->vnav, sizeof(kbb_display.vnav));
kbb_display.temperature = msg->temperature;
int32_t * i32buf = (int32_t*)kbb_display.__pad;
i32buf[0] = AA;
i32buf[1] = BB;
i32buf[2] = CC;
uint8_t * buf = (uint8_t*) &kbb_display;
kbb_display.header.checksum = calc_checksum_16(buf+KBB_CHECKSUM_START,
KBB_DISPLAY_SIZE-KBB_CHECKSUM_START);
// display!!!
int size_written = sdWrite(ed_serialPort, buf, KBB_DISPLAY_SIZE);
if ( size_written != KBB_DISPLAY_SIZE )
{
kbg_toggleLED3();
ASSERT(size_written==KBB_DISPLAY_SIZE,"thEDisplay", "Wrong sent data!!");
}
}
return;
}
void sendData(SerialDriver *SD, char id, char* message){
//static char data[12];
char checksum = id;
data[0] = 0xFF;
data[1] = 0xFF;
data[2] = id;
for(int i = 0; i< message[0]; i++){
data[i+3] = message[i];
checksum += message[i];
}
data[message[0]+3] = ~checksum;
sdWrite(SD, (uint8_t *) data, message[0]+4);
}
int _write_r(struct _reent *r, int file, char * ptr, int len)
{
(void)r;
(void)file;
(void)ptr;
#if defined(STDOUT_SD)
if (file != 1) {
__errno_r(r) = EINVAL;
return -1;
}
sdWrite(&STDOUT_SD, (uint8_t *)ptr, (size_t)len);
#endif
return len;
}
/**
* TX handler
*/
static void handle_uart_tx(struct uart_periph *p)
{
// check if more data to send
struct SerialInit *init_struct = (struct SerialInit*)(p->init_struct);
chSemWait (init_struct->tx_sem);
while (p->tx_insert_idx != p->tx_extract_idx) {
uint8_t data = p->tx_buf[p->tx_extract_idx];
p->tx_running = true;
sdWrite((SerialDriver *)p->reg_addr, &data, sizeof(data));
p->tx_running = false;
// TODO send by block (be careful with circular buffer)
chMtxLock(init_struct->tx_mtx);
p->tx_extract_idx++;
p->tx_extract_idx %= UART_TX_BUFFER_SIZE;
chMtxUnlock(init_struct->tx_mtx);
}
}
void
vexLcdSendMessage( LcdData *lcd, int16_t line )
{
// create message
int16_t i, cs;
// bounds check line variable
if( ( line < 0 ) || (line > 1) )
return;
// Header for LCD communication
lcd->txbuf[0] = 0xAA;
lcd->txbuf[1] = 0x55;
lcd->txbuf[2] = 0x1e;
lcd->txbuf[3] = 0x12;
lcd->txbuf[4] = (lcd->flags & VEX_LCD_BACKLIGHT) ? 0x02 + line : line;
// fill with spaces
for(i=0;i<16;i++)
lcd->txbuf[ 5+i ] = 0x20;
// Copy data transmit buffer
if(!line) {
for(i=0;i<16;i++)
if( lcd->line1[i] != 0)
lcd->txbuf[ 5+i ] = lcd->line1[i];
}
else {
for(i=0;i<16;i++) {
if( lcd->line2[i] != 0)
lcd->txbuf[ 5+i ] = lcd->line2[i];
}
}
// calculate checksum
cs = 0;
for(i=4;i<21;i++)
cs = cs + lcd->txbuf[i];
lcd->txbuf[21] = 0x100 - cs;
// send to port
sdWrite( lcd->sdp, (unsigned char *)lcd->txbuf, 22);
}
static void EM411Commands(void)
{
char nmeaMsg[140]; /* Store NMEA messages */
/*
* Disable unneeded messages from gps module.
* 0=GGA,1=GLL,2=GSA,3=GSV,4=RMC,5=VTG
*/
/*
* Enable GGA
*/
strcpy(nmeaMsg, "$PSRF103,00,00,01,01*00\r\n");
EM411NMEAChecksum(nmeaMsg);
sdWrite(&SD2, (uint8_t*) nmeaMsg, strlen(nmeaMsg));
/*
* Disable GLL
*/
strcpy(nmeaMsg, "$PSRF103,01,00,00,01*00\r\n");
EM411NMEAChecksum(nmeaMsg);
sdWrite(&SD2, (uint8_t*) nmeaMsg, strlen(nmeaMsg));
/*
* Disable GSA
*/
strcpy(nmeaMsg, "$PSRF103,02,00,00,01*00\r\n");
EM411NMEAChecksum(nmeaMsg);
sdWrite(&SD2, (uint8_t*) nmeaMsg, strlen(nmeaMsg));
/*
* Disable GSV
*/
strcpy(nmeaMsg, "$PSRF103,03,00,00,01*00\r\n");
EM411NMEAChecksum(nmeaMsg);
sdWrite(&SD2, (uint8_t*) nmeaMsg, strlen(nmeaMsg));
/*
* Disable RMC
*/
strcpy(nmeaMsg, "$PSRF103,04,00,00,01*00\r\n");
EM411NMEAChecksum(nmeaMsg);
sdWrite(&SD2, (uint8_t*) nmeaMsg, strlen(nmeaMsg));
/*
* Enable VTG
*/
strcpy(nmeaMsg, "$PSRF103,05,00,01,01*00\r\n");
EM411NMEAChecksum(nmeaMsg);
sdWrite(&SD2, (uint8_t*) nmeaMsg, strlen(nmeaMsg));
}
/*
* Application entry point.
*/
int main(void) {
/*
* 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();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* Creating blinkers threads.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO, Thread2, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the buttons state and run test procedure.
*/
while (TRUE) {
if (!palReadPad(IOPORT1, PA_BUTTON1))
sdWrite(&SD1, (uint8_t *)"Hello World!\r\n", 14);
if (!palReadPad(IOPORT1, PA_BUTTON2))
TestThread(&SD1);
chThdSleepMilliseconds(500);
}
return 0;
}
/**
* @brief Send a single SYNC Frame via serial.
*
*/
void DLLSendSyncFrame(DLLDriver *driver){
driver->DLLStats.SyncFrameSentCounter++;
sdWrite(driver->config->SDriver, driver->DLLSyncFrame, FRAME_SIZE_BYTE);
}
int wifiInit(void){
sdWrite(&SD3, &comAT,strlen(comAT));
chThdSleepMilliseconds(10000);
sdWrite(&SD3, &comATCWMODE,strlen(comATCWMODE));
chThdSleepMilliseconds(10000);
//sdWrite(&SD3, &comATCWLAP,strlen(comATCWLAP));
//chThdSleepMilliseconds(10000);
sdWrite(&SD3, &comATCWJAP,strlen(comATCWJAP));
sdWrite(&SD3, &sep,strlen(1));
sdWrite(&SD3, &SSID,strlen(SSID));
sdWrite(&SD3, &sep,strlen(1));
sdWrite(&SD3, &dot,strlen(dot));
sdWrite(&SD3, &sep,strlen(1));
sdWrite(&SD3, &password,strlen(password));
sdWrite(&SD3, &sep,strlen(1));
sdWrite(&SD3, &end,strlen(end));
chThdSleepMilliseconds(10000);
sdWrite(&SD3, &comATCWMODE,strlen(comATCWMODE));
chThdSleepMilliseconds(10000);
sdWrite(&SD3, &comATCIPMUX,strlen(comATCIPMUX));
chThdSleepMilliseconds(10000);
sdWrite(&SD3, &comATCIPSERVER,strlen(comATCIPSERVER));
chThdSleepMilliseconds(10000);
sdWrite(&SD3, &comAtCIFSR,strlen(comAtCIFSR));
}
void telnetReconfig(void){
sdWrite(&SD3, &comATCIPMUX,strlen(comATCIPMUX));
chThdSleepMilliseconds(5000);
}
int serialSend( uint8_t * data, int sz )
{
int cnt = sdWrite( &SERIAL_UART, (const uint8_t *)data, sz );
return cnt;
}
/*
* @brief console ...
*/
void Console::write(char *fmt, ...) {
chMtxLock(&this->_mutex);
char buffer[this->_buffer_size];
char *str = buffer;
va_list ap;
char *p, *s, c, filler;
int i, precision, width;
bool_t is_long, left_align;
long l;
#if CHPRINTF_USE_FLOAT
float f;
char tmpbuf[2 * MAX_FILLER + 1];
#else
char tmpbuf[MAX_FILLER + 1];
#endif
// print timestamp
uint8_t len = 0;
len = chsprintf(buffer, "[%d] ", chTimeNow());
sdWrite(this->_drv, (uint8_t *)buffer, len);
//
va_start(ap, fmt);
while (TRUE) {
c = *fmt++;
if (c == 0) {
va_end(ap);
break;
}
if (c != '%') {
*str++ = (uint8_t)c;
continue;
}
p = tmpbuf;
s = tmpbuf;
left_align = FALSE;
if (*fmt == '-') {
fmt++;
left_align = TRUE;
}
filler = ' ';
if (*fmt == '.') {
fmt++;
filler = '0';
}
width = 0;
while (TRUE) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
width = width * 10 + c;
}
precision = 0;
if (c == '.') {
while (TRUE) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
precision *= 10;
precision += c;
}
}
bool write(const void* ptr, uint32_t size)
{
return sdWrite(mSD, (uint8_t*)ptr, size) == size;
}
static msg_t GPSThread(void *arg) {
(void)arg;
chRegSetThreadName("gps_thread");
if (gps_data != NULL) {
chHeapFree(gps_data);
}
gps_data = chHeapAlloc(NULL, GPS_CMD_BUF);
size_t gps_bytes_read;
uint16_t i;
if (gps_data == NULL) {
while (TRUE) {
palTogglePad(GPIO_LED_1_PORT, GPIO_LED_1_PIN);
chThdSleepMilliseconds(50);
}
}
sdStart(&GPS_SERIAL, &SD3_Config);
palSetPadMode(GPS_USART_PORT, GPS_USART_TX_PIN, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPS_USART_PORT, GPS_USART_RX_PIN, PAL_MODE_ALTERNATE(7));
gps_reset();
while (TRUE) {
size_t readed_msg_len = 0;
uint8_t res = gps_read_msg(&readed_msg_len);
if (res == E_OK) {
if (check_checksum() != E_OK) {
sdWrite(&SD1, "GPS CHK ERR\r\n", 13);
} else {
if (gps_message_type() == GPS_MESSAGE_GPRMC) {
gps_rmc_state_t state;
//sdWrite(&SD1, gps_data, readed_msg_len);
if (parse_gps_rmc(&state) == E_OK) {
sdWrite(&SD1, "GPS PARSE OK\r\n", 14);
}
} else if (gps_message_type() == GPS_MESSAGE_UNKNOWN) {
sdWrite(&SD1, "GPS MSG UNKNOWN\r\n", 17);
}
}
} else {
gps_data[0] = '0' + res;
sdWrite(&SD1, "GPS ERROR:", 10);
sdWrite(&SD1, gps_data, 1);
gps_reset();
}
sdWrite(&SD1, "\r\n", 2);
//sdWrite(&SD1, "GPS: ", 5);
//while ((gps_bytes_read = sdReadTimeout(&GPS_SERIAL, gps_data, GPS_CMD_BUF, 100)) > 0)
// sdWrite(&SD1, gps_data, gps_bytes_read);
//sdWrite(&SD1, "|||\r\n", 5);
//chThdSleepMilliseconds(500);
}
chHeapFree(gps_data);
}
/*
* 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);
}
}
}
