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;
}
}
}
}
