void BaseWedgeI2CBus::open() { dev_.open(); selectedPort_ = NO_PORT; verifyBus(); initBus(); VLOG(4) << "successfully opened wedge CP2112 I2C bus"; }
void main() { byte i; long l; initBus(); initADC(); initI2C(); #ifdef HAS_UART initInterruptUarts(); #endif for(i=0; i<16; i++) cfgRegs[i] = 65; byte i2cErrCount = 0; while(1) { byte rx = readTemp(0x9E); /* Read error */ if(rx == 255) { if(i2cErrCount < 10) i2cErrCount++; else myTemperature = 255; initI2C(); } else { i2cErrCount = 0; myTemperature = rx; } } }
void main() { byte writeIndex = 0; byte i; long l; TRISB = 0xFFFF; /* All inputs */ LAT_BATT1_CTL = BATT_ENABLE; LAT_BATT2_CTL = BATT_ENABLE; LAT_BATT3_CTL = BATT_ENABLE; LAT_BATT4_CTL = BATT_ENABLE; #ifdef BBR2 LAT_BATT5_CTL = ~BATT_ENABLE; #else LAT_BATT5_CTL = BATT_ENABLE; #endif LAT_BATT6_CTL = BATT_ENABLE; TRIS_BATT1_CTL = TRIS_OUT; TRIS_BATT2_CTL = TRIS_OUT; TRIS_BATT3_CTL = TRIS_OUT; TRIS_BATT4_CTL = TRIS_OUT; #ifdef BBR2 TRIS_BATT5_CTL = TRIS_IN; #else TRIS_BATT5_CTL = TRIS_OUT; #endif TRIS_BATT6_CTL = TRIS_OUT; TRIS_BATT1 = TRIS_IN; TRIS_BATT2 = TRIS_IN; TRIS_BATT3 = TRIS_IN; TRIS_BATT4 = TRIS_IN; TRIS_BATT5 = TRIS_IN; TRIS_BATT6 = TRIS_IN; TRIS_WTRSEN = TRIS_IN; LAT_PWRKILL = ~PWRKILL_ON; TRIS_PWRKILL = TRIS_OUT; TRIS_LED_STA = TRIS_OUT; TRIS_LED_BATTLOW = TRIS_OUT; LAT_LED_STA = LED_ON; LAT_LED_BATTLOW = LED_ON; initBus(); LAT_LED_STA = LED_ON; LAT_LED_BATTLOW = ~LED_ON; // while(1); initADC(); initI2C(); initBattlowLight(); #ifdef HAS_UART initInterruptUarts(); #endif for(l=0; l<50000; l++); LAT_LED_STA = LED_ON; LAT_LED_BATTLOW = ~LED_ON; for(l=0; l<50000; l++); // LAT_PWRKILL = PWRKILL_ON; LAT_LED_STA = ~LED_ON; for(i=0; i<16; i++) cfgRegs[i] = 65; byte i2cErrCount = 0; while(1) { // checkBus(); /* Give it a second */ // for(l=0; l<10000; l++); byte rx = readTemp(0x9E); /* Read error */ if(rx == 255) { if(i2cErrCount < 10) i2cErrCount++; else myTemperature = 255; initI2C(); } else { i2cErrCount = 0; myTemperature = rx; } static const byte vADCs[]={ADC_B1V, ADC_B2V, ADC_B3V, ADC_B4V, ADC_B5V, ADC_B6V, ADC_26V}; /* Measure battery voltages */ for(i=0; i < 7; i++) { #ifdef BBR2 /* There is no fifth battery in BBR2, so skip it */ if(i == 4) { vBatt[4]= 0; continue; } #endif setADC(vADCs[i]); vBatt[i] = applyCalibration(readADC(), CAL_V_A, CAL_V_B); } /* Maintain running averages of the I sensors */ for(i=0; i < BATT_COUNT; i++) { #ifdef BBR2 /* There is no fifth battery in BBR2, so skip it */ if(i == 4) { iADCVal[4][writeIndex] = 0; continue; } #endif setADC(iADCs[i]); iADCVal[i][writeIndex] = readADC(); } writeIndex++; if(writeIndex >= IHISTORY_SIZE) writeIndex = 0; /* Calculate running averages of the battery currents */ for(i=0; i < BATT_COUNT; i++) { #ifdef BBR2 /* There is no fifth battery in BBR2, so skip it. */ if(i == 4) { iBatt[4] = 0; continue; } #endif iBatt[i] = applyCalibration(avgRow(i), CAL_I12V_A, CAL_I12V_B); } } }
int main(void) { long j=0; // long t=0, b=0; byte i; // byte tmp[60]; // byte rxPtr = 0; // byte rxLen = 0; TRIS_KS = TRIS_IN; initBus(); for(i=0; i<NUM_SLAVES; i++) setReq(i, 0); ADPCFG = 0xFFFF; LATB = 0; TRISB = 0; initMasterUart(); initInterruptUarts(); for(j=0; j<25000; j++); unsigned char emptyLine[]=" "; showString(emptyLine, 0); showString(emptyLine, 1); for(j=0; j<25000; j++); showString("Diagnostic?", 0); for(j=0; j<25000 && ((pollStatus() & 0x80) == 0); j++); if(pollStatus() & 0x80) diagBootMode(); showString("Starting up... ", 0); showString(" ", 1); while(1) { byte c = waitchar(0); long t1, t2; switch(c) { case HOST_CMD_SYNC: { sendByte(HOST_REPLY_SUCCESS); break; } case HOST_CMD_PING: { t1 = waitchar(1); if(t1 == HOST_CMD_PING) sendByte(HOST_REPLY_SUCCESS); else sendByte(HOST_REPLY_BADCHKSUM); break; } case HOST_CMD_SYSCHECK: { byte err=0; t1 = waitchar(1); if(t1 != HOST_CMD_SYSCHECK) { sendByte(HOST_REPLY_BADCHKSUM); break; } for(i=0; i<NUM_SLAVES; i++) { switch(busWriteByte(BUS_CMD_PING, i)) { case BUS_ERROR: err++; break; case BUS_FAILURE: err++; break; case 0: { byte len = readDataBlock(i); switch(len) { case 0: break; case BUS_ERROR: case BUS_FAILURE: default: err++; } } break; } } if(err == 0) sendByte(HOST_REPLY_SUCCESS); else sendByte(HOST_REPLY_FAILURE); break; } case HOST_CMD_DEPTH: { t1 = waitchar(1); if(t1 != HOST_CMD_DEPTH) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(BUS_CMD_DEPTH, SLAVE_ID_DEPTH) != 0) { sendByte(HOST_REPLY_FAILURE); break; } int len = readDataBlock(SLAVE_ID_DEPTH); if(len != 2) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_DEPTH); sendByte(rxBuf[0]); sendByte(rxBuf[1]); byte cs = HOST_REPLY_DEPTH+rxBuf[0]+rxBuf[1]; sendByte(cs); break; } case HOST_CMD_THRUSTERSTATE: { t1 = waitchar(1); if(t1 != HOST_CMD_THRUSTERSTATE) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(BUS_CMD_THRUSTER_STATE, SLAVE_ID_THRUSTERS) != 0) { sendByte(HOST_REPLY_FAILURE); break; } int len = readDataBlock(SLAVE_ID_THRUSTERS); if(len != 1) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_THRUSTERSTATE); sendByte(rxBuf[0]); byte cs = HOST_REPLY_THRUSTERSTATE+rxBuf[0]; sendByte(cs); break; } case HOST_CMD_BOARDSTATUS: { t1 = waitchar(1); if(t1 != HOST_CMD_BOARDSTATUS) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(BUS_CMD_BOARDSTATUS, SLAVE_ID_POWERBOARD) != 0) { sendByte(HOST_REPLY_FAILURE); break; } byte len = readDataBlock(SLAVE_ID_POWERBOARD); if(len!=1) { sendByte(HOST_REPLY_FAILURE); } else { rxBuf[0] &= 0xFD; // Clear kill switch bit // Set kill switch bit based on the GPIO kill input if(IN_KS == 1) rxBuf[0] |= 0x02; sendByte(HOST_REPLY_BOARDSTATUS); sendByte(rxBuf[0]); sendByte(HOST_REPLY_BOARDSTATUS+rxBuf[0]); } break; } case HOST_CMD_HARDKILL: { for(i=0; i<5; i++) rxBuf[i] = waitchar(1); byte cflag=0; for(i=0; i<5; i++) { if(rxBuf[i] != hkSafety[i]) cflag=1; } if(cflag == 1) { sendByte(HOST_REPLY_BADCHKSUM); break; } else { if(busWriteByte(BUS_CMD_HARDKILL, SLAVE_ID_HARDKILL) != 0) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_SUCCESS); } break; } case HOST_CMD_MARKER: { t1 = waitchar(1); t2 = waitchar(1); if((t1 != 0 && t1 != 1) || (t1+HOST_CMD_MARKER != t2)) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(t1==0 ? BUS_CMD_MARKER1 : BUS_CMD_MARKER2, SLAVE_ID_MARKERS) != 0) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_SUCCESS); break; } case HOST_CMD_BACKLIGHT: { t1 = waitchar(1); t2 = waitchar(1); const static unsigned char blCommands[]= {BUS_CMD_LCD_LIGHT_OFF, BUS_CMD_LCD_LIGHT_ON, BUS_CMD_LCD_LIGHT_FLASH}; if((t1 != 0 && t1 != 1 && t1 != 2) || (t1+HOST_CMD_BACKLIGHT != t2)) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(blCommands[t1], SLAVE_ID_LCD) != 0) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_SUCCESS); break; } case HOST_CMD_THRUSTERS: { for(i=0; i<5; i++) rxBuf[i] = waitchar(1); t1 = waitchar(1); t2 = waitchar(1); byte cflag=0; byte cs=0; // Check the special sequence for(i=0; i<5; i++) { cs += rxBuf[i]; if(rxBuf[i] != tkSafety[i]) cflag=1; } cs += t1 + HOST_CMD_THRUSTERS; const static unsigned char tkCommands[]= { BUS_CMD_THRUSTER1_OFF, BUS_CMD_THRUSTER2_OFF, BUS_CMD_THRUSTER3_OFF, BUS_CMD_THRUSTER4_OFF, BUS_CMD_THRUSTER1_ON, BUS_CMD_THRUSTER2_ON, BUS_CMD_THRUSTER3_ON, BUS_CMD_THRUSTER4_ON }; if(cflag == 1 || t1 > 7 || (t2 != cs)) { sendByte(HOST_REPLY_BADCHKSUM); break; } else { if(busWriteByte(tkCommands[t1], SLAVE_ID_THRUSTERS) != 0) { sendByte(HOST_REPLY_FAILURE); break; } } sendByte(HOST_REPLY_SUCCESS); break; } case HOST_CMD_TEMPERATURE: { t1 = waitchar(1); if(t1 != HOST_CMD_TEMPERATURE) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(BUS_CMD_TEMP, SLAVE_ID_TEMP) != 0) { sendByte(HOST_REPLY_FAILURE); break; } int len = readDataBlock(SLAVE_ID_TEMP); if(len != 5) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_TEMPERATURE); byte cs=0; for(i=0; i<5; i++) { cs += rxBuf[i]; sendByte(rxBuf[i]); } sendByte(cs + HOST_REPLY_TEMPERATURE); break; } case HOST_CMD_PRINTTEXT: { t1 = waitchar(1); byte cs=HOST_CMD_PRINTTEXT+t1; for(i=0; i<16; i++) { rxBuf[i] = waitchar(1); cs += rxBuf[i]; } t2 = waitchar(1); if(t2 != cs || t1 > 1) { sendByte(HOST_REPLY_BADCHKSUM); break; } int err=0; for(i=0; i<16 && err==0; i++) { err+=busWriteByte(BUS_CMD_LCD_WRITE, SLAVE_ID_LCD); err+=busWriteByte(t1*16+i, SLAVE_ID_LCD); err+=busWriteByte(rxBuf[i], SLAVE_ID_LCD); } err+=busWriteByte(BUS_CMD_LCD_REFRESH, SLAVE_ID_LCD); if(err != 0) sendByte(HOST_REPLY_FAILURE); else sendByte(HOST_REPLY_SUCCESS); break; } case HOST_CMD_SONAR: { t1 = waitchar(1); byte cs=HOST_CMD_SONAR+t1; if(t1 != HOST_CMD_SONAR) { sendByte(HOST_REPLY_BADCHKSUM); break; } if(busWriteByte(BUS_CMD_SONAR, SLAVE_ID_SONAR) != 0) { sendByte(HOST_REPLY_FAILURE); break; } int len = readDataBlock(SLAVE_ID_SONAR); if(len != 5) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_REPLY_SONAR); cs=0; for(i=0; i<5; i++) { cs += rxBuf[i]; sendByte(rxBuf[i]); } sendByte(cs + HOST_REPLY_SONAR); break; } case HOST_CMD_RUNTIMEDIAG: { t1 = waitchar(1); t2 = waitchar(1); if((t1 != 0 && t1 != 1) || (t1+HOST_CMD_RUNTIMEDIAG != t2)) { sendByte(HOST_REPLY_BADCHKSUM); break; } diagMsg=t1; if(t1==0) showString("Runtime Diag Off", 1); else showString("Runtime Diag On ", 1); sendByte(HOST_REPLY_SUCCESS); break; } case HOST_CMD_SETSPEED: { t1 = 0; /* Error counter */ /* 12 bytes of speed, plus checksum */ for(i=0; i<9; i++) rxBuf[i] = waitchar(1); for(i=0; i<8; i++) t1 += rxBuf[i]; t1 += HOST_CMD_SETSPEED; if(rxBuf[8] != (t1 & 0xFF)) { sendByte(HOST_REPLY_BADCHKSUM); break; } failsafeExpired = 0; /* Reset failsafe mechanism */ t1 = 0; if(busWriteByte(SLAVE_MM1_WRITE_CMD, SLAVE_ID_MM1) != 0) t1++; if(busWriteByte(rxBuf[0], SLAVE_ID_MM1) != 0) t1++; if(busWriteByte(rxBuf[1], SLAVE_ID_MM1) != 0) t1++; if(busWriteByte(SLAVE_MM2_WRITE_CMD, SLAVE_ID_MM2) != 0) t1++; if(busWriteByte(rxBuf[2], SLAVE_ID_MM2) != 0) t1++; if(busWriteByte(rxBuf[3], SLAVE_ID_MM2) != 0) t1++; if(busWriteByte(SLAVE_MM3_WRITE_CMD, SLAVE_ID_MM3) != 0) t1++; if(busWriteByte(rxBuf[4], SLAVE_ID_MM3) != 0) t1++; if(busWriteByte(rxBuf[5], SLAVE_ID_MM3) != 0) t1++; UARTSendSpeed(U2_MM_ADDR, rxBuf[6], rxBuf[7], 1); if(t1 == 0) sendByte(HOST_REPLY_SUCCESS); else sendByte(HOST_REPLY_FAILURE); break; } case HOST_CMD_MOTOR_READ: { unsigned char resp[4]; t1 = waitchar(1); if(t1 != HOST_CMD_MOTOR_READ) { sendByte(HOST_REPLY_BADCHKSUM); break; } t1 = 0; if(busWriteByte(SLAVE_MM1_READ_CMD, SLAVE_ID_MM1) != 0) t1++; if(readDataBlock(SLAVE_ID_MM1) != 1) t1++; resp[0] = rxBuf[0]; if(busWriteByte(SLAVE_MM2_READ_CMD, SLAVE_ID_MM2) != 0) t1++; if(readDataBlock(SLAVE_ID_MM2) != 1) t1++; resp[1] = rxBuf[0]; if(busWriteByte(SLAVE_MM3_READ_CMD, SLAVE_ID_MM3) != 0) t1++; if(readDataBlock(SLAVE_ID_MM3) != 1) t1++; resp[2] = rxBuf[0]; if(U2CanRead()) resp[3] = U2ReadByte(); else resp[3] = 0xFF; if(t1 != 0) { sendByte(HOST_REPLY_FAILURE); break; } sendByte(HOST_CMD_MOTOR_REPLY); sendByte(resp[0]); sendByte(resp[1]); sendByte(resp[2]); sendByte(resp[3]); sendByte(HOST_CMD_MOTOR_REPLY + resp[0] + resp[1] + resp[2] + resp[3]); break; } } } }