void beep(uint8_t number_of_beeps){
while (number_of_beeps){
R_PCLBUZ0_Start();
delayMillis(20);
R_PCLBUZ0_Stop();
number_of_beeps--;
if (number_of_beeps) delayMillis(40);
}
}
void testlines(uint16_t color)
{
uint16_t x,y;
fillScreen(BLACK);
for ( x=0; x < width(); x+=6)
{
drawLine(0, 0, x, height()-1, color);
}
// delayMillis(1500);
for ( y=0; y < height(); y+=6)
{
drawLine(0, 0, width()-1, y, color);
// delayMillis(500);
}
delayMillis(1500);
fillScreen(BLACK);
for ( x=0; x < width(); x+=6)
{
drawLine(width()-1, 0, x, height()-1, color);
// delayMillis(500);
}
for ( y=0; y < height(); y+=6)
{
drawLine(width()-1, 0, 0, y, color);
// delayMillis(500);
}
delayMillis(1500);
fillScreen(BLACK);
for ( x=0; x < width(); x+=6)
{
drawLine(0, height()-1, x, 0, color);
}
for ( y=0; y < height(); y+=6)
{
drawLine(0, height()-1, width()-1, y, color);
}
delayMillis(1500);
fillScreen(BLACK);
for ( x=0; x < width(); x+=6)
{
drawLine(width()-1, height()-1, x, 0, color);
}
for ( y=0; y < height(); y+=6)
{
drawLine(width()-1, height()-1, 0, y, color);
}
// printf("Done testlines\r\n");
}
// random chase
void randomchase(void) {
int m;
for ( m = 0; m <= NUMLEDS; m++ ) {
setPixelS(m, adcGenRand24());
setPixelS(m-1, clear);
display();
delayMillis(100);
}
for ( m = NUMLEDS; m >= 0; m-- ) {
setPixelS(m, adcGenRand24());
setPixelS(m+1, clear);
display();
delayMillis(100);
}
}
void ADXL345::calibrate(GPIO light)
{
zeroX = 0;
zeroY = 0;
zeroZ = 0;
vector v;
for (int i = 0; i < 1000; i++)
{
if (i % 15 == 0)
light.toggle();
read(v);
zeroX += v.x;
zeroY += v.y;
zeroZ += (1-v.z);
delayMillis(5);
}
light.low();
zeroX /= 1000;
zeroY /= 1000;
zeroZ /= 1000;
calibrated = 1;
}
void drawSmlLogo(void *pvParameters) {
while (1) {
BSP_LCD_Clear(LCD_COLOR_BLACK);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_WHITE);
BSP_LCD_FillCircle(100, 72, 10);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_DARKBLUE);
BSP_LCD_FillRect(90, 87, 60, 20);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_LIGHTBLUE);
BSP_LCD_FillCircle(140, 122, 10);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_DARKMAGENTA);
BSP_LCD_FillRect(90, 137, 60, 20);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_LIGHTMAGENTA);
BSP_LCD_FillRect(80, 162, 60, 20);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_MAGENTA);
BSP_LCD_FillRect(90, 187, 60, 20);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_DARKGREEN);
BSP_LCD_FillRect(90, 212, 60, 20);
logoElementsDelay();
BSP_LCD_SetTextColor(LCD_COLOR_GREEN);
BSP_LCD_FillCircle(100, 247, 10);
delayMillis(1000);
}
}
void CDC_TestSend(void) {
if (USB_PacketSent) {
int tCount = snprintf(StringBuffer, sizeof StringBuffer, "Test %p", &StringBuffer[0]);
CDC_Send_DATA((uint8_t *) &StringBuffer[0], tCount);
delayMillis(100);
}
}
bool RegisterIO::GetConfiguration() {
bool success = false;
int retry_count = 0;
while ( retry_count < 3 && !success ) {
char config[NAVX_REG_SENSOR_STATUS_H+1] = {0};
if ( io_provider->Read(NAVX_REG_WHOAMI, (uint8_t *)config, sizeof(config)) ) {
board_id.hw_rev = config[NAVX_REG_HW_REV];
board_id.fw_ver_major = config[NAVX_REG_FW_VER_MAJOR];
board_id.fw_ver_minor = config[NAVX_REG_FW_VER_MINOR];
board_id.type = config[NAVX_REG_WHOAMI];
notify_sink->SetBoardID(board_id);
board_state.cal_status = config[NAVX_REG_CAL_STATUS];
board_state.op_status = config[NAVX_REG_OP_STATUS];
board_state.selftest_status = config[NAVX_REG_SELFTEST_STATUS];
board_state.sensor_status = IMURegisters::decodeProtocolUint16(config + NAVX_REG_SENSOR_STATUS_L);
board_state.gyro_fsr_dps = IMURegisters::decodeProtocolUint16(config + NAVX_REG_GYRO_FSR_DPS_L);
board_state.accel_fsr_g = (int16_t)config[NAVX_REG_ACCEL_FSR_G];
board_state.update_rate_hz = config[NAVX_REG_UPDATE_RATE_HZ];
board_state.capability_flags = IMURegisters::decodeProtocolUint16(config + NAVX_REG_CAPABILITY_FLAGS_L);
notify_sink->SetBoardState(board_state);
success = true;
} else {
success = false;
delayMillis(50);
}
retry_count++;
}
return success;
}
/**
* TASK 2: Detect Button Press And Signal Event via Inter-Process Communication (IPC)
*/
void detectButtonPress(void *pvParameters) {
int sig = 1;
while (1) {
/* Detect Button Press */
if (BSP_PB_GetState(BUTTON_KEY) > 0) {
/* Debounce */
while (BSP_PB_GetState(BUTTON_KEY) > 0)
delayMillis(100);
while (BSP_PB_GetState(BUTTON_KEY) == 0)
delayMillis(100);
xQueueSendToBack(buttonPushesQueue, &sig, 0); /* Send Message */
}
}
}
// display static rainbow
void showrainbow(void) {
int k;
for ( k=0; k < NUMLEDS; k++ ) {
setPixelS(k, wheel( ((k * 256 / NUMLEDS )) % 255) );
}
display();
delayMillis(100);
}
void USB_ChangeToJoystick(void) {
if (isUSBTypeCDC()) {
USB_PowerOff();
delayMillis(50);
pProperty = &Device_Property_Joystick; // since pointer is used by reset
Device_Property_Joystick.Reset();
USB_Init_Dynamic(&Device_Property_Joystick, &User_Standard_Requests_Joystick);
}
}
void initDisplay(void)
{
uint16_t a, d;
uint8_t i;
rotation = 0;
_width = TFTWIDTH;
_height = TFTHEIGHT;
cursor_y = cursor_x = 0;
textsize = 1;
textcolor = 0xFFFF;
GPIO_SetBits(GPIOE, GPIO_Pin_1);
delayMillis(2);
reset();
uint16_t identifier = readRegister(0x0);
printf ("TFTLCD Driver ID: 0x%04X\r\n",identifier);
if (identifier == 0x9325)
{
printf("Found ILI9325");
} else if (identifier == 0x9328) {
printf("Found ILI9328\r\n");
} else {
printf("Unknown driver chip\r\n");
return;
}
for (i = 0; i < sizeof(_regValues) / 4; i++) {
a = *(_regValues + i*2);
d = *(_regValues + i*2 + 1);
if (a == 0xFF) {
delayMillis(d);
} else {
writeRegister(a, d);
//printf("addr: %d data: 0x%04X\r\n",a,d);
}
}
}
void USB_ChangeToCDC(void) {
if (!isUSBTypeCDC()) {
USB_PowerOff();
// TODO which delay is needed - test it
delayMillis(10);
pProperty = &Device_Property_CDC; // since pointer is used by reset
Device_Property_CDC.Reset();
USB_Init_Dynamic(&Device_Property_CDC, &User_Standard_Requests_CDC); // calls USB_PowerOn();
}
}
/**
* Call this to start up HAL. This is required for robot programs.
*/
int HALInitialize(int mode)
{
setlinebuf(stdin);
setlinebuf(stdout);
prctl(PR_SET_PDEATHSIG, SIGTERM);
FRC_NetworkCommunication_Reserve(nullptr);
// image 4; Fixes errors caused by multiple processes. Talk to NI about this
nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
nLoadOut::kTargetClass_RoboRIO;
int32_t status = 0;
global = tGlobal::create(&status);
watchdog = tSysWatchdog::create(&status);
// Kill any previous robot programs
std::fstream fs;
// By making this both in/out, it won't give us an error if it doesnt exist
fs.open("/var/lock/frc.pid", std::fstream::in | std::fstream::out);
if (fs.bad())
return 0;
pid_t pid = 0;
if (!fs.eof() && !fs.fail())
{
fs >> pid;
//see if the pid is around, but we don't want to mess with init id=1, or ourselves
if (pid >= 2 && kill(pid, 0) == 0 && pid != getpid())
{
std::cout << "Killing previously running FRC program..."
<< std::endl;
kill(pid, SIGTERM); // try to kill it
delayMillis(100);
if (kill(pid, 0) == 0)
{
// still not successfull
if (mode == 0)
{
std::cout << "FRC pid " << pid
<< " did not die within 110ms. Aborting"
<< std::endl;
return 0; // just fail
}
else if (mode == 1) // kill -9 it
kill(pid, SIGKILL);
else
{
std::cout << "WARNING: FRC pid " << pid
<< " did not die within 110ms." << std::endl;
}
}
}
}
// animate fading rainbow cycle
void rainbowcycle(void) {
int k, j;
for ( j=0; j<256; j++ ) {
for ( k=0; k < NUMLEDS; k++ ) {
setPixelS(k, wheel( ( k+j) % 255 ) );
}
display();
delayMillis(100);
}
}
int main(void)
{
initSystick();
unsigned int pin = 13; // pin 13 is the orange LED
RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN; // enable the clock to GPIOD
GPIOD->MODER = (1 << 2*pin); // set pin 13 to be general purpose output
while (1) {
delayMillis(1000);
GPIOD->ODR ^= (1 << pin); // Toggle pin
setbuf(stdout, NULL);
printf("Toggling LED from off to on\r\n"); //waiting 1000 millisecond to turn on the LED
delayMillis(1000);
GPIOD->ODR ^= (1<<pin);
setbuf(stdout,NULL);
printf("Toggling LED from on to off\r\n"); //waiting 1000 millisecond to turn off the LED
}
}
// red white and blue chasers
void goJoe(unsigned long time) {
int m;
colorwipe(clear); // clear display from existing patterns
for ( m = 0; m < NUMLEDS; m++ ) {
setPixelS(m, blue);
setPixelS(m - 2, red);
setPixelS(m - 4, white);
setPixelS(m - 6, clear);
display();
delayMillis(time);
}
for ( m = NUMLEDS; m >= 0; m-- ) {
setPixelS(m, clear);
setPixelS(m - 2, white);
setPixelS(m - 4, red);
setPixelS(m - 6, blue);
display();
delayMillis(time);
}
}
/**
* @brief This function handles the test program fail.
* @param None
* @retval None
*/
void Fail_Handler(void)
{
// /* Erase last sector */
// FLASH_EraseSector(FLASH_Sector_11, VoltageRange_3);
// /* Write FAIL code at last word in the flash memory */
// FLASH_ProgramWord(TESTRESULT_ADDRESS, ALLTEST_FAIL);
while(1)
{
/* Toggle Red LED */
STM_EVAL_LEDToggle(LED5);
delayMillis(50);
}
}
//LBGG color chase
void lbgg_chase(void) {
int m;
int time = analog_val; //TODO: do some math here
//colorwipe(clear); do this in switch logic
for ( m = 0; m < NUMLEDS; m++ ) {
setPixelS(m, green);
setPixelS(m - 2, white);
setPixelS(m - 4, green);
setPixelS(m - 6, clear);
display();
delayMillis(time);
}
for ( m = NUMLEDS; m >= 0; m-- ) {
setPixelS(m, clear);
setPixelS(m - 2, green);
setPixelS(m - 4, white);
setPixelS(m - 6, green);
display();
delayMillis(time);
}
}
void reset(void)
{
GPIO_ResetBits(GPIOE, GPIO_Pin_1);
// digitalWrite(_reset, LOW);
delayMillis(2);
// digitalWrite(_reset, HIGH);
GPIO_SetBits(GPIOE, GPIO_Pin_1);
printf("Reset TFT - \n\r");
// resync
writeData(0);
writeData(0);
writeData(0);
writeData(0);
}
void RegisterIO::Run() {
io_provider->Init();
/* Initial Device Configuration */
SetUpdateRateHz(this->update_rate_hz);
GetConfiguration();
/* IO Loop */
while (!stop) {
if ( board_state.update_rate_hz != this->update_rate_hz ) {
SetUpdateRateHz(this->update_rate_hz);
}
GetCurrentData();
delayMillis(1000.0/this->update_rate_hz);
}
}
void doIRButtons(TouchButton * const aTheTouchedButton, int16_t aValue) {
FeedbackToneOK();
if (aTheTouchedButton == TouchButtonsIRSend[0]) {
sIRSendData.protocol = IRMP_NEC_PROTOCOL; // use NEC protocol
sIRSendData.address = 64260; // set address LG 0XFB04
sIRSendData.command = 0x00FF; // set command to EZ Adjust
sIRSendData.flags = 0; // don't repeat frame
//irsnd_send_data(&sIRSendData, TRUE); // send frame, wait for completion
irsnd_send_data(&sIRSendData, FALSE); // send frame, do not wait for completion
} else if (aTheTouchedButton == TouchButtonsIRSend[1]) {
sIRSendData.protocol = IRMP_NEC_PROTOCOL; // use NEC protocol
sIRSendData.address = 64260; // set address
sIRSendData.command = 251; // set command to In Start
sIRSendData.flags = 0;
irsnd_send_data(&sIRSendData, TRUE); // send frame, wait for completion
} else if (aTheTouchedButton == TouchButtonsIRSend[2]) {
float tNumber = getNumberFromNumberPad(NUMBERPAD_DEFAULT_X, 0, COLOR_GREEN);
if (!isnan(tNumber) && tNumber != IN_STOP_CODE) {
sCode = tNumber;
}
displayIRPage();
assertParamMessage((sCode != IN_STOP_CODE), sCode, "");
if (sCode != IN_STOP_CODE) {
sIRSendData.protocol = IRMP_NEC_PROTOCOL; // use NEC protocol
sIRSendData.address = 64260; // set address
sIRSendData.command = sCode; // set command to
sIRSendData.flags = 1; // repeat frame once
irsnd_send_data(&sIRSendData, TRUE); // send frame, wait for completion
}
} else if (aTheTouchedButton == TouchButtonsIRSend[3]) {
sIRSendData.protocol = IRMP_NEC_PROTOCOL; // use NEC protocol
sIRSendData.address = 64260; // set address
sIRSendData.command = sCode; // set command to Volume -
sIRSendData.flags = 0x0F; // repeat frame forever
irsnd_send_data(&sIRSendData, FALSE); // send frame, do not wait for completion
delayMillis(2000);
irsnd_stop();
}
snprintf(StringBuffer, sizeof StringBuffer, "Cmd=%4d|%3X", sCode, sCode);
BlueDisplay1.drawText(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_5_LINE_2 + TEXT_SIZE_22_HEIGHT, StringBuffer, TEXT_SIZE_22,
COLOR_PAGE_INFO, COLOR_WHITE);
}
void display_right(void) {
unsigned long data;
// send all the pixels
for ( p=0; p < NUMLEDS_R ; p++ ) {
data = pixels_right[p];
// 24 bits of data per pixel
for ( i=0x800000; i>0 ; i>>=1 ) {
if (data & i) {
P1OUT |= DATA_R;
} else {
P1OUT &= ~DATA_R;
}
P1OUT |= CLOCK_R; // latch on clock rise
P1OUT &= ~CLOCK_R;
}
}
writezeros(3 * ((NUMLEDS_R + 63) / 64)); // latch
delayMillis(3);
}
void ADXL345::init(I2C* I2Cx)
{
i2c = I2Cx;
i2c->sendByte(ADXL345_ADDRESS, ADXL345_REG_POWER_CTL, 0x08);
//set range to 8+-
uint8_t data;
i2c->readByte(ADXL345_ADDRESS, ADXL345_REG_DATA_FORMAT, data);
data &= ~0x0F;
data |= ADXL345_RANGE_8_G;
data |= 0x08;
i2c->sendByte(ADXL345_ADDRESS, ADXL345_REG_DATA_FORMAT, data);
//set datarate
i2c->sendByte(ADXL345_ADDRESS, ADXL345_REG_BW_RATE, ADXL345_DATARATE_200_HZ);
calibrated = 0;
delayMillis(250);
}
static void imuTask(IMU *imu)
{
char stream_type;
uint16_t gyro_fsr_dps, accel_fsr, update_rate_hz;
uint16_t q1_offset, q2_offset, q3_offset, q4_offset;
float yaw_offset_degrees;
uint16_t flags;
bool stream_response_received = false;
double last_valid_packet_time = 0.0;
int partial_binary_packet_count = 0;
int stream_response_receive_count = 0;
int stream_response_timeout_count = 0;
int timeout_count = 0;
int discarded_bytes_count = 0;
int port_reset_count = 0;
double last_stream_command_sent_timestamp = 0.0;
int updates_in_last_second = 0;
double last_second_start_time = 0;
SerialPort *pport = imu->GetSerialPort();
try {
pport->SetReadBufferSize(512);
pport->SetTimeout(1.0);
pport->EnableTermination('\n');
pport->Flush();
pport->Reset();
} catch(std::exception ex) {
}
int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer,
imu->current_stream_type, imu->update_rate_hz );
try {
pport->Reset();
pport->Write( protocol_buffer, cmd_packet_length );
pport->Flush();
#ifdef DEBUG_IMU_RX
port_reset_count++;
SmartDashboard::PutNumber("nav6_PortResets", (double)port_reset_count);
#endif
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
} catch (std::exception ex) {
}
while (!stop) {
try {
// Wait, with delays to conserve CPU resources, until
// bytes have arrived.
while ( !stop && ( pport->GetBytesReceived() < 1 ) ) {
delayMillis(1000/update_rate_hz);
}
int packets_received = 0;
uint32_t bytes_read = pport->Read( protocol_buffer, 256 );
if (bytes_read > 0) {
byte_count += bytes_read;
uint32_t i = 0;
// Scan the buffer looking for valid packets
while (i < bytes_read) {
// Attempt to decode a packet
int bytes_remaining = bytes_read - i;
if ( protocol_buffer[i] != PACKET_START_CHAR ) {
/* Skip over received bytes until a packet start is detected. */
i++;
#ifdef DEBUG_IMU_RX
discarded_bytes_count++;
SmartDashboard::PutNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
#endif
continue;
} else {
if ( ( bytes_remaining > 2 ) &&
( protocol_buffer[i+1] == '#' ) ) {
/* Binary packet received; next byte is packet length-2 */
uint8_t total_expected_binary_data_bytes = protocol_buffer[i+2];
total_expected_binary_data_bytes += 2;
while ( bytes_remaining < total_expected_binary_data_bytes ) {
/* This binary packet contains an embedded */
/* end-of-line character. Continue to receive */
/* more data until entire packet is received. */
uint32_t additional_received_data_length = pport->Read(additional_received_data, 256);
if ( additional_received_data_length > 0 ) {
byte_count += additional_received_data_length;
memcpy(protocol_buffer + bytes_read, additional_received_data, additional_received_data_length);
bytes_read += additional_received_data_length;
bytes_remaining += additional_received_data_length;
} else {
/* Timeout waiting for remainder of binary packet */
i++;
bytes_remaining--;
#ifdef DEBUG_IMU_RX
partial_binary_packet_count++;
SmartDashboard::PutNumber("nav6 Partial Binary Packets", (double)partial_binary_packet_count);
#endif
continue;
}
}
}
}
int packet_length = imu->DecodePacketHandler(&protocol_buffer[i],bytes_remaining);
if (packet_length > 0) {
packets_received++;
update_count++;
last_valid_packet_time = Timer::GetFPGATimestamp();
updates_in_last_second++;
if ((last_valid_packet_time - last_second_start_time ) > 1.0 ) {
#ifdef DEBUG_IMU_RX
SmartDashboard::PutNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
updates_in_last_second = 0;
#endif
last_second_start_time = last_valid_packet_time;
}
i += packet_length;
}
else
{
packet_length = IMUProtocol::decodeStreamResponse( &protocol_buffer[i], bytes_remaining, stream_type,
gyro_fsr_dps, accel_fsr, update_rate_hz,
yaw_offset_degrees,
q1_offset, q2_offset, q3_offset, q4_offset,
flags );
if ( packet_length > 0 )
{
packets_received++;
imu->SetStreamResponse( stream_type,
gyro_fsr_dps, accel_fsr, update_rate_hz,
yaw_offset_degrees,
q1_offset, q2_offset, q3_offset, q4_offset,
flags );
stream_response_received = true;
i += packet_length;
#ifdef DEBUG_IMU_RX
stream_response_receive_count++;
SmartDashboard::PutNumber("nav6 Stream Responses", (double)stream_response_receive_count);
#endif
}
else {
// current index is not the start of a valid packet; increment
i++;
#ifdef DEBUG_IMU_RX
discarded_bytes_count++;
SmartDashboard::PutNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
#endif
}
}
}
if ( ( packets_received == 0 ) && ( bytes_read == 256 ) ) {
// Workaround for issue found in SerialPort implementation:
// No packets received and 256 bytes received; this
// condition occurs in the SerialPort. In this case,
// reset the serial port.
pport->Reset();
#ifdef DEBUG_IMU_RX
port_reset_count++;
SmartDashboard::PutNumber("nav6_PortResets", (double)port_reset_count);
#endif
}
// If a stream configuration response has not been received within three seconds
// of operation, (re)send a stream configuration request
if ( !stream_response_received && ((Timer::GetFPGATimestamp() - last_stream_command_sent_timestamp ) > 3.0 ) ) {
int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz );
try {
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
pport->Write( protocol_buffer, cmd_packet_length );
pport->Flush();
} catch (std::exception ex2) {
#ifdef DEBUG_IMU_RX
stream_response_timeout_count++;
SmartDashboard::PutNumber("nav6 Stream Response Timeouts", (double)stream_response_timeout_count);
#endif
}
}
else {
// If no bytes remain in the buffer, and not awaiting a response, sleep a bit
if ( stream_response_received && ( pport->GetBytesReceived() == 0 ) ) {
delayMillis(1000/update_rate_hz);
}
}
/* If receiving data, but no valid packets have been received in the last second */
/* the navX MXP may have been reset, but no exception has been detected. */
/* In this case , trigger transmission of a new stream_command, to ensure the */
/* streaming packet type is configured correctly. */
if ( ( Timer::GetFPGATimestamp() - last_valid_packet_time ) > 1.0 ) {
stream_response_received = false;
}
}
} catch (std::exception ex) {
// This exception typically indicates a Timeout
stream_response_received = false;
#ifdef DEBUG_IMU_RX
timeout_count++;
SmartDashboard::PutNumber("nav6 Serial Port Timeouts", (double)timeout_count);
SmartDashboard::PutString("LastNavException", ex.what());
#endif
}
}
}
void SerialIO::Run() {
stop = false;
bool stream_response_received = false;
double last_stream_command_sent_timestamp = 0.0;
double last_data_received_timestamp = 0;
double last_second_start_time = 0;
int partial_binary_packet_count = 0;
int stream_response_receive_count = 0;
int timeout_count = 0;
int discarded_bytes_count = 0;
int port_reset_count = 0;
int updates_in_last_second = 0;
int integration_response_receive_count = 0;
try {
serial_port->SetReadBufferSize(256);
serial_port->SetTimeout(1.0);
serial_port->EnableTermination('\n');
serial_port->Flush();
serial_port->Reset();
} catch (std::exception ex) {
printf("SerialPort Run() Port Initialization Exception: %s\n", ex.what());
}
char stream_command[256];
char integration_control_command[256];
IMUProtocol::StreamResponse response = {0};
AHRSProtocol::IntegrationControl integration_control = {0};
AHRSProtocol::IntegrationControl integration_control_response = {0};
int cmd_packet_length = IMUProtocol::encodeStreamCommand( stream_command, update_type, update_rate_hz );
try {
serial_port->Reset();
serial_port->Write( stream_command, cmd_packet_length );
cmd_packet_length = AHRSProtocol::encodeDataGetRequest( stream_command, AHRS_DATA_TYPE::BOARD_IDENTITY, AHRS_TUNING_VAR_ID::UNSPECIFIED );
serial_port->Write( stream_command, cmd_packet_length );
serial_port->Flush();
port_reset_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Port Resets", (double)port_reset_count);
#endif
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
} catch (std::exception ex) {
printf("SerialPort Run() Port Send Encode Stream Command Exception: %s\n", ex.what());
}
int remainder_bytes = 0;
char received_data[256 * 3];
char additional_received_data[256];
char remainder_data[256];
while (!stop) {
try {
// Wait, with delays to conserve CPU resources, until
// bytes have arrived.
if ( signal_transmit_integration_control ) {
integration_control.action = next_integration_control_action;
signal_transmit_integration_control = false;
next_integration_control_action = 0;
cmd_packet_length = AHRSProtocol::encodeIntegrationControlCmd( integration_control_command, integration_control );
try {
serial_port->Write( integration_control_command, cmd_packet_length );
} catch (std::exception ex) {
printf("SerialPort Run() IntegrationControl Send Exception: %s\n", ex.what());
}
}
if ( !stop && ( remainder_bytes == 0 ) && ( serial_port->GetBytesReceived() < 1 ) ) {
delayMillis(1000/update_rate_hz);
}
int packets_received = 0;
int bytes_read = serial_port->Read(received_data, sizeof(received_data));
byte_count += bytes_read;
/* If a partial packet remains from last iteration, place that at */
/* the start of the data buffer, and append any new data available */
/* at the serial port. */
if ( remainder_bytes > 0 ) {
memcpy( received_data + bytes_read, remainder_data, remainder_bytes);
bytes_read += remainder_bytes;
remainder_bytes = 0;
}
if (bytes_read > 0) {
last_data_received_timestamp = Timer::GetFPGATimestamp();
int i = 0;
// Scan the buffer looking for valid packets
while (i < bytes_read) {
// Attempt to decode a packet
int bytes_remaining = bytes_read - i;
if ( received_data[i] != PACKET_START_CHAR ) {
/* Skip over received bytes until a packet start is detected. */
i++;
discarded_bytes_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Discarded Bytes", (double)discarded_bytes_count);
#endif
continue;
} else {
if ( ( bytes_remaining > 2 ) &&
( received_data[i+1] == BINARY_PACKET_INDICATOR_CHAR ) ) {
/* Binary packet received; next byte is packet length-2 */
uint8_t total_expected_binary_data_bytes = received_data[i+2];
total_expected_binary_data_bytes += 2;
while ( bytes_remaining < total_expected_binary_data_bytes ) {
/* This binary packet contains an embedded */
/* end-of-line character. Continue to receive */
/* more data until entire packet is received. */
int additional_received_data_length =
serial_port->Read(additional_received_data,sizeof(additional_received_data));
byte_count += additional_received_data_length;
/* Resize array to hold existing and new data */
if ( additional_received_data_length > 0 ) {
memcpy( received_data + bytes_remaining, additional_received_data, additional_received_data_length);
bytes_remaining += additional_received_data_length;
} else {
/* Timeout waiting for remainder of binary packet */
i++;
bytes_remaining--;
partial_binary_packet_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Partial Binary Packets", (double)partial_binary_packet_count);
#endif
continue;
}
}
}
}
int packet_length = DecodePacketHandler(received_data + i,bytes_remaining);
if (packet_length > 0) {
packets_received++;
update_count++;
last_valid_packet_time = Timer::GetFPGATimestamp();
updates_in_last_second++;
if ((last_valid_packet_time - last_second_start_time ) > 1.0 ) {
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Updates Per Sec", (double)updates_in_last_second);
#endif
updates_in_last_second = 0;
last_second_start_time = last_valid_packet_time;
}
i += packet_length;
}
else
{
packet_length = IMUProtocol::decodeStreamResponse(received_data + i, bytes_remaining, response);
if (packet_length > 0) {
packets_received++;
DispatchStreamResponse(response);
stream_response_received = true;
i += packet_length;
stream_response_receive_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Stream Responses", (double)stream_response_receive_count);
#endif
}
else {
packet_length = AHRSProtocol::decodeIntegrationControlResponse( received_data + i, bytes_remaining,
integration_control_response );
if ( packet_length > 0 ) {
// Confirmation of integration control
integration_response_receive_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Integration Control Response Count", integration_response_receive_count);
#endif
i += packet_length;
} else {
/* Even though a start-of-packet indicator was found, the */
/* current index is not the start of a packet if interest. */
/* Scan to the beginning of the next packet, */
bool next_packet_start_found = false;
int x;
for ( x = 0; x < bytes_remaining; x++ ) {
if ( received_data[i + x] != PACKET_START_CHAR) {
x++;
} else {
i += x;
bytes_remaining -= x;
if ( x != 0 ) {
next_packet_start_found = true;
}
break;
}
}
bool discard_remainder = false;
if ( !next_packet_start_found && x == bytes_remaining ) {
/* Remaining bytes don't include a start-of-packet */
discard_remainder = true;
}
bool partial_packet = false;
if ( discard_remainder ) {
/* Discard the remainder */
i = bytes_remaining;
} else {
if ( !next_packet_start_found ) {
/* This occurs when packets are received that are not decoded. */
/* Bump over this packet and prepare for the next. */
if ( ( bytes_remaining > 2 ) &&
( received_data[i+1] == BINARY_PACKET_INDICATOR_CHAR ) ) {
/* Binary packet received; next byte is packet length-2 */
int pkt_len = received_data[i+2];
pkt_len += 2;
if ( bytes_remaining >= pkt_len ) {
bytes_remaining -= pkt_len;
i += pkt_len;
discarded_bytes_count += pkt_len;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Discarded Bytes", (double)discarded_bytes_count);
#endif
} else {
/* This is the initial portion of a partial binary packet. */
/* Keep this data and attempt to acquire the remainder. */
partial_packet = true;
}
} else {
/* Ascii packet received. */
/* Scan up to and including next end-of-packet character */
/* sequence, or the beginning of a new packet. */
for ( x = 0; x < bytes_remaining; x++ ) {
if ( received_data[i+x] == '\r') {
i += x+1;
bytes_remaining -= (x+1);
discarded_bytes_count += x+1;
if ( ( bytes_remaining > 0 ) && received_data[i] == '\n') {
bytes_remaining--;
i++;
discarded_bytes_count++;
}
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Discarded Bytes", (double)discarded_bytes_count);
#endif
break;
}
/* If a new start-of-packet is found, discard */
/* the ascii packet bytes that precede it. */
if ( received_data[i+x] == '!') {
if ( x > 0 ) {
i += x;
bytes_remaining -= x;
discarded_bytes_count += x;
break;
} else {
/* start of packet found, but no termination */
/* Time to get some more data, unless the bytes */
/* remaining are larger than a valid packet size */
if ( bytes_remaining < IMU_PROTOCOL_MAX_MESSAGE_LENGTH ) {
/* Get more data */
partial_packet = true;
} else {
i++;
bytes_remaining--;
}
break;
}
}
}
if ( x == bytes_remaining ) {
/* Partial ascii packet - keep the remainder */
partial_packet = true;
}
}
}
}
if ( partial_packet ) {
if ( bytes_remaining > (int)sizeof(remainder_data)) {
memcpy(remainder_data, received_data + i - sizeof(remainder_data), sizeof(remainder_data));
remainder_bytes = sizeof(remainder_data);
} else {
memcpy(remainder_data, received_data + i, bytes_remaining);
remainder_bytes = bytes_remaining;
}
i = bytes_read;
}
}
}
}
}
if ( ( packets_received == 0 ) && ( bytes_read == 256 ) ) {
// Workaround for issue found in SerialPort implementation:
// No packets received and 256 bytes received; this
// condition occurs in the SerialPort. In this case,
// reset the serial port.
serial_port->Flush();
serial_port->Reset();
port_reset_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Port Resets", (double)port_reset_count);
#endif
}
bool retransmit_stream_config = false;
if ( signal_retransmit_stream_config ) {
retransmit_stream_config = true;
signal_retransmit_stream_config = false;
}
// If a stream configuration response has not been received within three seconds
// of operation, (re)send a stream configuration request
if ( retransmit_stream_config ||
(!stream_response_received && ((Timer::GetFPGATimestamp() - last_stream_command_sent_timestamp ) > 3.0 ) ) ) {
cmd_packet_length = IMUProtocol::encodeStreamCommand( stream_command, update_type, update_rate_hz );
try {
ResetSerialPort();
last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
serial_port->Write( stream_command, cmd_packet_length );
cmd_packet_length = AHRSProtocol::encodeDataGetRequest( stream_command, AHRS_DATA_TYPE::BOARD_IDENTITY, AHRS_TUNING_VAR_ID::UNSPECIFIED );
serial_port->Write( stream_command, cmd_packet_length );
serial_port->Flush();
} catch (std::exception ex2) {
printf("SerialPort Run() Re-transmit Encode Stream Command Exception: %s\n", ex2.what());
}
}
else {
// If no bytes remain in the buffer, and not awaiting a response, sleep a bit
if ( stream_response_received && ( serial_port->GetBytesReceived() == 0 ) ) {
delayMillis(1000/update_rate_hz);
}
}
/* If receiving data, but no valid packets have been received in the last second */
/* the navX MXP may have been reset, but no exception has been detected. */
/* In this case , trigger transmission of a new stream_command, to ensure the */
/* streaming packet type is configured correctly. */
if ( ( Timer::GetFPGATimestamp() - last_valid_packet_time ) > 1.0 ) {
last_stream_command_sent_timestamp = 0.0;
stream_response_received = false;
}
} else {
/* No data received this time around */
if ( Timer::GetFPGATimestamp() - last_data_received_timestamp > 1.0 ) {
ResetSerialPort();
}
}
} catch (std::exception ex) {
// This exception typically indicates a Timeout, but can also be a buffer overrun error.
stream_response_received = false;
timeout_count++;
#ifdef SERIALIO_DASHBOARD_DEBUG
SmartDashboard::PutNumber("navX Serial Port Timeout / Buffer Overrun", (double)timeout_count);
SmartDashboard::PutString("navX Last Exception", ex.what());
#endif
ResetSerialPort();
}
}
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
uint8_t currentLED=0,idx;
uint16_t xx,yy;
uint16_t testWord;
uint8_t shifter=0;
/* Initialize LEDs on STM32F4-Discovery --------------------*/
__IO uint32_t i = 0;
uint8_t buf[255];
uint8_t len;
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED3);
// You can use these for LEDs if you use the upper 8 bits of the 16b FSMC bus to talk to the ILI9238.
// STM_EVAL_LEDInit(LED5); //GPIOD 14 -> FSMC D0. Do not use for LED, we need them to talk to the ILI9238
// STM_EVAL_LEDInit(LED6); //GPIOD 14 -> FSMC D1.
// flash the LEDs in a circle to test delayMillis(uint32_t timedelay)
STM_EVAL_LEDToggle(currentLED);
for (idx=0;idx<8;idx++)
{
STM_EVAL_LEDToggle(currentLED);
currentLED=(currentLED+1)%2;
STM_EVAL_LEDToggle(currentLED);
delayMillis(250);
}
//
// USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
// init the printf
init_printf(0,tft_putc);
// init_tft_printf(NULL,tft_putc);
// Get the 32F4 ready to talk to the TFTLCD using FSMC
initGPIO();
initFSMC();
uDelay(1000); // probably don't need this
reset();
initDisplay();
// ** Do the adafruit Demo **
// fillScreen(BLACK);
// setCursor(0, 0);
// setTextColor(CYAN);
// setTextSize(1);
// setRotation(1);
// tft_printf("Please connect to virtual COM port...");
delayMillis(2000);
testlines(CYAN);
delayMillis(2500);
testfastlines(RED, BLUE);
delayMillis(2500);
testdrawrects(GREEN);
delayMillis(2500);
testfillrects(YELLOW, MAGENTA);
delayMillis(2500);
fillScreen(BLACK);
testfillcircles(10, MAGENTA);
testdrawcircles(10, WHITE);
delayMillis(2500);
testtriangles();
delayMillis(2500);
testfilltriangles();
delayMillis(2500);
testRoundRect();
delayMillis(2500);
testFillRoundRect();
delayMillis(2500);
fillScreen(GREEN);
delayMillis(2500);
// fsmcData = 0x60020000; // sets a16
// fsmcRegister = 0x60000000; // clears a16
// printf("fsmcData:\t0x%08X\r\n",fsmcData);
// printf("fsmcRegister:\t0x%08X\r\n",fsmcRegister);
// fillScreen(BLACK);
// setCursor(0, 20);
// setTextColor(color);
// setTextSize(1);
// write("Hello World!");
// setTextSize(2);
// write(1234.56);
// setTextSize(3);
//
// println(0xDEADBEEF, HEX);
// printf("0xFF00>>8 0x%04X\r\n",0xff00>>8);
// VCP_send_str(&buf[0]);
// printf("SysTick_Config(SystemCoreClock/1000)\t %d\r\n",SysTick_Config(SystemCoreClock/1000));
// millisecondCounter=0;
// for (idx=0;idx<100;idx++)
// {
// printf("millisecondCounter:\t%d",millisecondCounter);
// }
// void delayMillis(uint32_t millis)
// {
// uint32_t target;
//
// target=millisecondCounter+millis;
// while(millisecondCounter<target);
// }
// From stm32f4_discovery.h:
// typedef enum
// {
// LED4 = 0,
// LED3 = 1,
// LED5 = 2,
// LED6 = 3
// } Led_TypeDef;
while(1)
{
for (idx=0;idx<8;idx++)
{
setRotation(idx%4);
testtext(RED);
delayMillis(1500);
}
}
}
/**
* TASK 1: Toggle LED via RTOS Timer
*/
void toggleLedWithTimer(void *pvParameters) {
while (1) {
BSP_LED_Toggle(LED3);
delayMillis(1500);
}
}
void solidblink(unsigned long c) {
colorwipe(c);
delayMillis(500);
colorwipe(clear);
delayMillis(500);
}
