void SensorInit(void){
uint32_t sensorSetFlag;
SENSOR_VERSION.word = 0x14081000LU;
SubsystemInit(SENSOR, MESSAGE, "SENSOR", SENSOR_VERSION);
//Enable register access to ADC0
SysCtlPeripheralEnable( SYSCTL_PERIPH_ADC0 );
//Enable register access to GPIO Port B
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
// Set the sensor outputs as outputs
GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_5 | GPIO_PIN_7);
// Select the analog ADC function for these pins.
GPIOPinTypeADC( GPIO_PORTB_BASE, GPIO_PIN_4); //Sensor 1 In
GPIOPinTypeADC( GPIO_PORTB_BASE, GPIO_PIN_6); //Sensor 2 In
//Set up the ADC sequencer
ADCSequenceConfigure(ADC0_BASE, 0, ADC_TRIGGER_ALWAYS, 3);
ADCSequenceStepConfigure(ADC0_BASE, 0, 0, ADC_CTL_CH10|ADC_CTL_CMP0);
ADCSequenceStepConfigure(ADC0_BASE, 0, 1, ADC_CTL_CH10|ADC_CTL_END);
ADCSequenceEnable(ADC0_BASE, 0);
// Read from EEPROM to see if this is the first time this is being setup
EEPROMRead(&sensorSetFlag, EE_ADDR_SENSOR_SETUP, sizeof(sensorSetFlag));
if(sensorSetFlag == EE_SENSOR_SET){
EEPROMRead(&sensorTimeout, EE_ADDR_SENSOR_TIMEOUT, sizeof(sensorTimeout));
EEPROMRead(&sensorThreshold, EE_ADDR_SENSOR_THRESHOLD, sizeof(sensorThreshold));
LogMsg(SENSOR, MESSAGE, "Sensor Timeout Restored : %k seconds", sensorTimeout/1000);
LogMsg(SENSOR, MESSAGE, "Sensor Threshold Restored To : %k", sensorThreshold);
} else {
sensorSetFlag = EE_SENSOR_SET;
EEPROMProgram(&sensorTimeout, EE_ADDR_SENSOR_TIMEOUT, sizeof(sensorTimeout));
EEPROMProgram(&sensorThreshold, EE_ADDR_SENSOR_THRESHOLD, sizeof(sensorThreshold));
EEPROMProgram(&sensorSetFlag, EE_ADDR_SENSOR_SETUP, sizeof(sensorSetFlag));
LogMsg(SENSOR, MESSAGE, "No sensor values saved, default values used. Threshold: %k, Timeout: %k", sensorThreshold, sensorTimeout);
}
//Set up the comparator
ADCComparatorConfigure(ADC0_BASE, 0, ADC_COMP_TRIG_NONE|ADC_COMP_INT_LOW_HONCE );
ADCComparatorRegionSet(ADC0_BASE, 0, sensorThreshold - SENSOR_HYST_WINDOW, sensorThreshold + SENSOR_HYST_WINDOW);
ADCComparatorReset(ADC0_BASE, 0, true, true);
ADCComparatorIntEnable(ADC0_BASE, 0);
ADCIntRegister(ADC0_BASE, 0, Sensor1ISR);
ADCIntEnable(ADC0_BASE, 0);
}
// Start up
void ESPHB::Startup(void){
EEPROMRead(0,&DV_INF);
EEPROMRead(50,&WF_INF);
if(DV_INF.DEBUG==1){
DEBUG = true;
}else{
DEBUG = false;
}
DEBUG = true;
if(DV_INF.FIRST_START!=1)
Restore(); // Set defaults on first startup
if(DV_INF.USE_DHT==1){
dht.init(DV_INF.DHT_PIN, DV_INF.DHT_TYPE, 11,DV_INF.DEBUG);
}
}
UINT16 __fastcall ddonpachReadWord(UINT32 sekAddress)
{
switch (sekAddress) {
case 0x300002: {
return YMZ280BReadStatus();
}
case 0x800000: {
UINT16 nRet = 6 | nVideoIRQ;
nVideoIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0x800002:
case 0x800004:
case 0x800006: {
UINT16 nRet = 6 | nVideoIRQ;
return nRet;
}
case 0xD00000:
return DrvInput[0] ^ 0xFFFF;
case 0xD00002:
return (DrvInput[1] ^ 0xF7FF) | (EEPROMRead() << 11);
default: {
// bprintf(PRINT_NORMAL, "Attempt to read word value of location %x\n", sekAddress);
}
}
return 0;
}
UINT16 __fastcall korokoroReadWord(UINT32 sekAddress)
{
switch (sekAddress) {
case 0x1c0000:
case 0x300000:
return (nUnknownIRQ << 1) | nVideoIRQ | (bVBlank ? 4 : 0);
case 0x1c0002:
case 0x300002:
return (nUnknownIRQ << 1) | nVideoIRQ;
case 0x1c0004:
case 0x300004:
nVideoIRQ = 1;
UpdateIRQStatus();
return (nUnknownIRQ << 1) | nVideoIRQ;
case 0x1c0006:
case 0x300006:
nUnknownIRQ = 1;
UpdateIRQStatus();
return (nUnknownIRQ << 1) | nVideoIRQ;
case 0x280000:
return (DrvInput[0] & 0x5FFF) | (korokoro_hopper ? 0x0000 : 0x8000) | ((DrvDip[0] & 0x20) << 8);
case 0x280002:
return (DrvInput[1] & 0xEFFF) | ((EEPROMRead() & 1) ? 0x1000 : 0x0000);
// default:
// bprintf(PRINT_NORMAL, "Attempt to read word value of location %x\n", sekAddress);
}
return 0;
}
//----------------------------------------------------------------------------//
void Bypass(void)
{
if(SwTimer4 >= 50 )
{
SwTimer4 = 0;
if(BypassCounter >= 5)
{
RLY_1 = 1;
EEPROMRead(&myDevicesRequiredStatus, 0, 1);
if(myDevicesRequiredStatus[0] == 0xAA) // estado relevador apagado
{
i = 0x55;
EEPROMWRITE(&i,0,1);
EEPROMWRITE(&i,0,1);
EEPROMWRITE(&i,0,1);
EEPROMWRITE(&i,0,1);
EEPROMWRITE(&i,0,1);
}
ConsolePutROMString((ROM char *)"\r\nBypass");
}
}
}
void SaveGains(){
uint8_t calibrationFlags;
uint16_t j = GAINS_START;
float_u outFloat;
j = GAINS_START;
for (uint16_t i = KP_PITCH_RATE_; i <= FC_CT_; i++) {
outFloat.val = *floatPointerArray[i];
EEPROMWrite(j++, outFloat.buffer[0]);
EEPROMWrite(j++, outFloat.buffer[1]);
EEPROMWrite(j++, outFloat.buffer[2]);
EEPROMWrite(j++, outFloat.buffer[3]);
}
j = DEC_START;
outFloat.val = *floatPointerArray[MAG_DEC_];
EEPROMWrite(j++, outFloat.buffer[0]);
EEPROMWrite(j++, outFloat.buffer[1]);
EEPROMWrite(j++, outFloat.buffer[2]);
EEPROMWrite(j++, outFloat.buffer[3]);
cosDec = cos(declination);
sinDec = sin(declination);
calibrationFlags = EEPROMRead(CAL_FLAGS);
calibrationFlags &= ~(1<<GAINS_FLAG);
EEPROMWrite(CAL_FLAGS,calibrationFlags);
RCFailSafeCounter = 0;
groundFSCount = 0;
GPSFailSafeCounter = 0;
baroFSCount = 0;
watchDogFailSafeCounter = 0;
}
UINT8 __fastcall hotdogstReadByte(UINT32 sekAddress)
{
switch (sekAddress) {
case 0xc80002:
return ((DrvInput[1] >> 8) ^ 0xF7) | (EEPROMRead() << 3);
default: {
bprintf(PRINT_NORMAL, _T("Attempt to read byte value of location %x\n"), sekAddress);
}
}
return 0;
}
UINT8 __fastcall korokoroReadByte(UINT32 sekAddress)
{
switch (sekAddress)
{
case 0x1c0000:
case 0x1c0001:
case 0x300000:
case 0x300001:
return (nUnknownIRQ << 1) | nVideoIRQ | (bVBlank ? 0 : 4);
case 0x1c0002:
case 0x1c0003:
case 0x300002:
case 0x300003:
return (nUnknownIRQ << 1) | nVideoIRQ;
case 0x1c0004:
case 0x1c0005:
case 0x300004:
case 0x300005:
nVideoIRQ = 1;
UpdateIRQStatus();
return (nUnknownIRQ << 1) | nVideoIRQ;
case 0x1c0006:
case 0x1c0007:
case 0x300006:
case 0x300007:
nUnknownIRQ = 1;
UpdateIRQStatus();
return (nUnknownIRQ << 1) | nVideoIRQ;
case 0x280000:
return ((DrvInput[0] >> 8) & 0x5F) | (korokoro_hopper ? 0x00 : 0x80) | (DrvDip[0] & 0x20);
case 0x280001:
return DrvInput[0] & 0xFF;
case 0x280002:
return ((DrvInput[1] >> 8) & 0xEF) | (EEPROMRead() ? 0x10 : 0x00);
case 0x280003:
return DrvInput[1] & 0xFF;
// default:
// bprintf(PRINT_NORMAL, "Attempt to read byte value of location %x\n", sekAddress);
}
return 0;
}
static UINT8 OthunderInputBypassRead(INT32 Offset)
{
switch (Offset) {
case 0x03: {
return (EEPROMRead() & 1) << 7;
}
default: {
return TC0220IOCRead(Offset);
}
}
return 0;
}
static unsigned char OthunderInputBypassRead(int Offset)
{
switch (Offset) {
case 0x03: {
return (EEPROMRead() & 1) << 7;
}
default: {
return TC0220IOCRead(Offset);
}
}
return 0;
}
void read_TM_KEY_settings(){
int i,y;
uint8_t temp;
temp = EEPROMRead(EEPROM_tms_numbers,1);
if (temp != 0xFE )tm_key_number = temp;
for (i = 0;i< tm_key_number;i++){
for (y = 0;y < 8; y++){
TM_KEY[i].id[y] = EEPROMRead_id((EEPROM_tms_id + (i * 8) + y));
}
for (y = 0;y < 10; y++){
TM_KEY[i].name[y] = EEPROMRead_id((EEPROM_tms_name + (i * 10) + y));
if (TM_KEY[i].name[y] == 0xFE) break;
}
}
}
UINT16 __fastcall metmqstrReadWord(UINT32 sekAddress)
{
switch (sekAddress) {
case 0xa80000:
case 0xa80002: {
UINT8 nRet = (nUnknownIRQ << 1) | nVideoIRQ;
return nRet;
}
case 0xa80004: {
UINT8 nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nVideoIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0xa80006: {
UINT8 nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nUnknownIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0xa8006C:
if (SoundLatchReplyIndex > SoundLatchReplyMax) {
return 2;
}
return 0;
case 0xa8006E:
if (SoundLatchReplyIndex > SoundLatchReplyMax) {
SoundLatchReplyIndex = 0;
SoundLatchReplyMax = -1;
return 0;
}
return SoundLatchReply[SoundLatchReplyIndex++];
case 0xc80000:
return DrvInput[0] ^ 0xFFFF;
case 0xc80002:
return (DrvInput[1] ^ 0xF7FF) | (EEPROMRead() << 11);
default: {
bprintf(PRINT_NORMAL, _T("Attempt to read word value of location %x\n"), sekAddress);
}
}
return 0;
}
unsigned char __fastcall donpachiReadByte(unsigned int sekAddress)
{
switch (sekAddress) {
case 0x900000:
case 0x900001:
case 0x900002:
case 0x900003: {
unsigned char nRet = (nUnknownIRQ << 1) | nVideoIRQ;
return nRet;
}
case 0x900004:
case 0x900005: {
unsigned char nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nVideoIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0x900006:
case 0x900007: {
unsigned char nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nUnknownIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0xB00001:
return MSM6295ReadStatus(0);
case 0xB00011:
return MSM6295ReadStatus(1);
case 0xC00000:
return (DrvInput[0] >> 8) ^ 0xFF;
case 0xC00001:
return (DrvInput[0] & 0xFF) ^ 0xFF;
case 0xC00002:
return (DrvInput[1] >> 8) ^ 0xF7 | (EEPROMRead() << 3);
case 0xC00003:
return (DrvInput[1] & 0xFF) ^ 0xFF;
default: {
// bprintf(PRINT_NORMAL, "Attempt to read byte value of location %x\n", sekAddress);
}
}
return 0;
}
void downloadSerialNumberFromEEPROM(void)
{
BYTE i;
char *dscrRAM;
BYTE xdata buf[MAX_NAME_LENGTH];
// get pointer to string descriptor 3
dscrRAM = (char *)EZUSB_GetStringDscr(3);
// read string description from EEPROM
EEPROMRead(STRING_ADDRESS, MAX_NAME_LENGTH, buf);
//write string description (serial number) to RAM
for (i=0;i<MAX_NAME_LENGTH;i++)
{
dscrRAM[2+i*2] = buf[i];
}
}
UINT8 __fastcall ddonpachReadByte(UINT32 sekAddress)
{
switch (sekAddress) {
case 0x300002:
case 0x300003: {
return YMZ280BReadStatus();
}
case 0x800000:
case 0x800001: {
UINT8 nRet = 6 | nVideoIRQ;
nVideoIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0x800002:
case 0x800003:
case 0x800004:
case 0x800005:
case 0x800006:
case 0x800007: {
UINT8 nRet = 6 | nVideoIRQ;
return nRet;
}
case 0xD00000:
return (DrvInput[0] >> 8) ^ 0xFF;
case 0xD00001:
return (DrvInput[0] & 0xFF) ^ 0xFF;
case 0xD00002:
return ((DrvInput[1] >> 8) ^ 0xF7) | (EEPROMRead() << 3);
case 0xD00003:
return (DrvInput[1] & 0xFF) ^ 0xFF;
default: {
// bprintf(PRINT_NORMAL, "Attempt to read byte value of location %x\n", sekAddress);
}
}
return 0;
}
unsigned short __fastcall donpachiReadWord(unsigned int sekAddress)
{
switch (sekAddress) {
case 0x900000:
case 0x900002: {
unsigned short nRet = (nUnknownIRQ << 1) | nVideoIRQ;
return nRet;
}
case 0x900004: {
unsigned short nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nVideoIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0x900006: {
unsigned short nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nUnknownIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0xB00000:
return MSM6295ReadStatus(0);
case 0xB00010:
return MSM6295ReadStatus(1);
case 0xC00000:
return DrvInput[0] ^ 0xFFFF;
case 0xC00002:
return DrvInput[1] ^ 0xF7FF | (EEPROMRead() << 11);
default: {
// bprintf(PRINT_NORMAL, "Attempt to read word value of location %x\n", sekAddress);
}
}
return 0;
}
UINT16 __fastcall hotdogstReadWord(UINT32 sekAddress)
{
switch (sekAddress) {
case 0xa80000:
case 0xa80002: {
UINT8 nRet = (nUnknownIRQ << 1) | nVideoIRQ;
return nRet;
}
case 0xa80004: {
UINT8 nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nVideoIRQ = 1;
UpdateIRQStatus();
return nRet;
}
case 0xa80006: {
UINT8 nRet = (nUnknownIRQ << 1) | nVideoIRQ;
nUnknownIRQ = 1;
UpdateIRQStatus();
return nRet;
}
// case 0xa8006e: {
// return 0xff;
// }
case 0xc80000:
return DrvInput[0] ^ 0xFFFF;
case 0xC80002:
return (DrvInput[1] ^ 0xF7FF) | (EEPROMRead() << 11);
default: {
bprintf(PRINT_NORMAL, _T("Attempt to read word value of location %x\n"), sekAddress);
}
}
return 0;
}
unsigned char __fastcall ps5_read_byte(unsigned int address)
{
address &= 0xc7ffffff;
switch (address)
{
case 0x3100000:
return BurnYMF278BReadStatus();
case 0x3000000:
case 0x3000001:
case 0x3000002:
case 0x3000003:
return DrvInputs >> ((~address & 3) << 3);
case 0x3000004:
return DrvDips[1] | ((EEPROMRead() & 1) << 4);
case 0x3100005:
return DrvSndROM[(sample_offs++) & 0x3fffff];
}
return 0;
}
//*****************************************************************************
//
// Main application entry point.
//
//*****************************************************************************
int main(void) {
int_fast32_t i32IPart[17], i32FPart[17];
uint_fast32_t ui32Idx, ui32CompDCMStarted;
float pfData[17];
float *pfAccel, *pfGyro, *pfMag, *pfEulers, *pfQuaternion;
float *direction;
//
// Initialize convenience pointers that clean up and clarify the code
// meaning. We want all the data in a single contiguous array so that
// we can make our pretty printing easier later.
//
pfAccel = pfData;
pfGyro = pfData + 3;
pfMag = pfData + 6;
pfEulers = pfData + 9;
pfQuaternion = pfData + 12;
direction = pfData + 16;
//
// Setup the system clock to run at 40 Mhz from PLL with crystal reference
//
ROM_SysCtlClockSet(
SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ
| SYSCTL_OSC_MAIN);
//
// Enable port E used for motion interrupt.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
//
// Enable port F used for calibration.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//
// Initialize the UART.
//
ConfigureUART();
/* EEPROM SETTINGS */
SysCtlPeripheralEnable(SYSCTL_PERIPH_EEPROM0); // EEPROM activate
EEPROMInit(); // EEPROM start
//
// Print the welcome message to the terminal.
//
UARTprintf("\033[2JMPU9150 Raw Example\n");
//
// Set the color to a purple approximation.
//
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x8000;
//
// Initialize RGB driver.
//
RGBInit(0);
RGBColorSet(g_pui32Colors);
RGBIntensitySet(0.5f);
RGBEnable();
// Initialize BGLib
bglib_output = output;
ConfigureBLE();
//
// The I2C3 peripheral must be enabled before use.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C3);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
//
// Configure the pin muxing for I2C3 functions on port D0 and D1.
//
ROM_GPIOPinConfigure(GPIO_PD0_I2C3SCL);
ROM_GPIOPinConfigure(GPIO_PD1_I2C3SDA);
//
// Select the I2C function for these pins. This function will also
// configure the GPIO pins pins for I2C operation, setting them to
// open-drain operation with weak pull-ups. Consult the data sheet
// to see which functions are allocated per pin.
//
GPIOPinTypeI2CSCL(GPIO_PORTD_BASE, GPIO_PIN_0);
ROM_GPIOPinTypeI2C(GPIO_PORTD_BASE, GPIO_PIN_1);
//
// Configure and Enable the GPIO interrupt. Used for INT signal from the
// MPU9150
//
ROM_GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_2);
GPIOIntEnable(GPIO_PORTE_BASE, GPIO_PIN_2);
ROM_GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_2, GPIO_FALLING_EDGE);
ROM_IntEnable(INT_GPIOE);
//
// Keep only some parts of the systems running while in sleep mode.
// GPIOE is for the MPU9150 interrupt pin.
// UART0 is the virtual serial port
// TIMER0, TIMER1 and WTIMER5 are used by the RGB driver
// I2C3 is the I2C interface to the ISL29023
//
ROM_SysCtlPeripheralClockGating(true);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_GPIOE);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_UART0);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER0);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER1);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_I2C3);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_WTIMER5);
//
// Enable interrupts to the processor.
//
ROM_IntMasterEnable();
//
// Initialize I2C3 peripheral.
//
I2CMInit(&g_sI2CInst, I2C3_BASE, INT_I2C3, 0xff, 0xff,
ROM_SysCtlClockGet());
//
// Initialize the MPU9150 Driver.
//
MPU9150Init(&g_sMPU9150Inst, &g_sI2CInst, MPU9150_I2C_ADDRESS,
MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Configure the sampling rate to 1000 Hz / (1+24).
//
g_sMPU9150Inst.pui8Data[0] = 24;
MPU9150Write(&g_sMPU9150Inst, MPU9150_O_SMPLRT_DIV, g_sMPU9150Inst.pui8Data,
1, MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Write application specifice sensor configuration such as filter settings
// and sensor range settings.
//
g_sMPU9150Inst.pui8Data[0] = MPU9150_CONFIG_DLPF_CFG_94_98;
g_sMPU9150Inst.pui8Data[1] = MPU9150_GYRO_CONFIG_FS_SEL_250;
g_sMPU9150Inst.pui8Data[2] = (MPU9150_ACCEL_CONFIG_ACCEL_HPF_5HZ
| MPU9150_ACCEL_CONFIG_AFS_SEL_2G);
// g_sMPU9150Inst.pui8Data[2] = MPU9150_ACCEL_CONFIG_AFS_SEL_2G;
MPU9150Write(&g_sMPU9150Inst, MPU9150_O_CONFIG, g_sMPU9150Inst.pui8Data, 3,
MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Configure the data ready interrupt pin output of the MPU9150.
//
g_sMPU9150Inst.pui8Data[0] = MPU9150_INT_PIN_CFG_INT_LEVEL
| MPU9150_INT_PIN_CFG_INT_RD_CLEAR
| MPU9150_INT_PIN_CFG_LATCH_INT_EN;
g_sMPU9150Inst.pui8Data[1] = MPU9150_INT_ENABLE_DATA_RDY_EN;
MPU9150Write(&g_sMPU9150Inst, MPU9150_O_INT_PIN_CFG,
g_sMPU9150Inst.pui8Data, 2, MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Initialize the DCM system. 40 hz sample rate.
// accel weight = .2, gyro weight = .8, mag weight = .2
//
CompDCMInit(&g_sCompDCMInst, 1.0f / 40.0f, 0.2f, 0.6f, 0.2f);
//
// Enable blinking indicates config finished successfully
//
RGBBlinkRateSet(1.0f);
//
// Configure and Enable the GPIO interrupt. Used for calibration
//
HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x01;
ROM_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_4);
GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_STRENGTH_2MA,
GPIO_PIN_TYPE_STD_WPU);
ROM_IntEnable(INT_GPIOF);
ROM_GPIOIntTypeSet(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_FALLING_EDGE);
GPIOIntEnable(GPIO_PORTF_BASE, GPIO_PIN_4);
g_calibrationState = 0;
ui32CompDCMStarted = 0;
// Configure the white noise, read the error from EEPROM
EEPROMRead((uint32_t *) zeroErrorAccel,
EEPROM_ZERO_ERROR_ACCELERATION_ADDRESS, 12);
EEPROMRead((uint32_t *) linearErrorAccel,
EEPROM_LINEAR_ERROR_ACCELERATION_ADDRESS, 12);
EEPROMRead((uint32_t *) zeroErrorGyro, EEPROM_ZERO_ERROR_GYROSCOPE_ADDRESS,
12);
while (1) {
//
// Go to sleep mode while waiting for data ready.
//
while (!g_vui8I2CDoneFlag) {
//ROM_SysCtlSleep();
}
//
// Clear the flag
//
g_vui8I2CDoneFlag = 0;
//
// Get floating point version of the Accel Data in m/s^2.
//
MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
pfAccel + 2);
//
// Get floating point version of angular velocities in rad/sec
//
MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
pfGyro + 2);
//
// Get floating point version of magnetic fields strength in tesla
//
MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
pfMag + 2);
if (g_calibrationState == 2) {
zeroErrorAccel[0] = (pfAccel[0]
+ zeroErrorAccel[0] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorAccel[1] = (pfAccel[1]
+ zeroErrorAccel[1] * g_calibrationCount)
/ (g_calibrationCount + 1);
accelAtGravity[2] = (pfAccel[2]
+ accelAtGravity[2] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorGyro[0] = (pfGyro[0]
+ zeroErrorGyro[0] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorGyro[1] = (pfGyro[1]
+ zeroErrorGyro[1] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorGyro[2] = (pfGyro[2]
+ zeroErrorGyro[2] * g_calibrationCount)
/ (g_calibrationCount + 1);
g_calibrationCount++;
if (g_calibrationCount > 500) {
Calibration();
}
continue;
} else if (g_calibrationState == 4) {
zeroErrorAccel[2] = (pfAccel[2]
+ zeroErrorAccel[2] * g_calibrationCount)
/ (g_calibrationCount + 1);
accelAtGravity[1] = (pfAccel[1]
+ accelAtGravity[1] * g_calibrationCount)
/ (g_calibrationCount + 1);
g_calibrationCount++;
if (g_calibrationCount > 500) {
Calibration();
}
continue;
} else if (g_calibrationState == 6) {
accelAtGravity[0] = (pfAccel[0]
+ accelAtGravity[0] * g_calibrationCount)
/ (g_calibrationCount + 1);
g_calibrationCount++;
if (g_calibrationCount > 500) {
Calibration();
}
continue;
}
// Cancel out white noise
// pfAccel[0] = pfAccel[0] - zeroErrorAccel[0];
// pfAccel[1] = pfAccel[1] - zeroErrorAccel[1];
// pfAccel[2] = pfAccel[2] - zeroErrorAccel[2];
// pfGyro[0] = pfGyro[0] - zeroErrorGyro[0];
// pfGyro[1] = pfGyro[1] - zeroErrorGyro[1];
// pfGyro[2] = pfGyro[2] - zeroErrorGyro[2];
// // Straighten out linear noise
// pfAccel[0] = pfAccel[0] * (1 + linearErrorAccel[0]);
// pfAccel[1] = pfAccel[1] * (1 + linearErrorAccel[1]);
// pfAccel[2] = pfAccel[2] * (1 + linearErrorAccel[2]);
//
// Check if this is our first data ever.
//
if (ui32CompDCMStarted == 0) {
//
// Set flag indicating that DCM is started.
// Perform the seeding of the DCM with the first data set.
//
ui32CompDCMStarted = 1;
CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1], pfMag[2]);
CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
pfAccel[2]);
CompDCMGyroUpdate(&g_sCompDCMInst, pfGyro[0], pfGyro[1], pfGyro[2]);
CompDCMStart(&g_sCompDCMInst);
} else {
//
// DCM Is already started. Perform the incremental update.
//
CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1], pfMag[2]);
CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
pfAccel[2]);
CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
-pfGyro[2]);
CompDCMUpdate(&g_sCompDCMInst);
}
//
// Increment the skip counter. Skip counter is used so we do not
// overflow the UART with data.
//
g_ui32PrintSkipCounter++;
if (g_ui32PrintSkipCounter >= PRINT_SKIP_COUNT) {
//
// Reset skip counter.
//
g_ui32PrintSkipCounter = 0;
//
// Get Euler data. (Roll Pitch Yaw)
//
CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
pfEulers + 2);
//
// Get Quaternions.
//
CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);
//
// convert mag data to micro-tesla for better human interpretation.
//
pfMag[0] *= 1e6;
pfMag[1] *= 1e6;
pfMag[2] *= 1e6;
//
// Convert Eulers to degrees. 180/PI = 57.29...
// Convert Yaw to 0 to 360 to approximate compass headings.
//
pfEulers[0] *= 57.295779513082320876798154814105f;
pfEulers[1] *= 57.295779513082320876798154814105f;
pfEulers[2] *= 57.295779513082320876798154814105f;
if (pfEulers[2] < 0) {
pfEulers[2] += 360.0f;
}
// Use pfMag to display degrees of the Magnetomer's x-axis
// (y-axis of accelerometer and gyroscope) to the east of
// magnetic north pole
// direction[0] = 0;
// if (pfMag[1] == 0) {
// if (pfMag[0] > 0) {
// direction[0] = 0;
// } else {
// direction[0] = 180;
// }
// } else if (pfMag[1] > 0) {
// direction[0] = 90 - atan2f(pfMag[0], pfMag[1]) * 180 / 3.14159265359;
// } else if (pfMag[1] < 0) {
// direction[0] = 270 - atan2f(pfMag[0], pfMag[1]) * 180 / 3.14159265359;
// }
//
// Now drop back to using the data as a single array for the
// purpose of decomposing the float into a integer part and a
// fraction (decimal) part.
//
for (ui32Idx = 0; ui32Idx < 17; ui32Idx++) {
//
// Conver float value to a integer truncating the decimal part.
//
i32IPart[ui32Idx] = (int32_t) pfData[ui32Idx];
//
// Multiply by 1000 to preserve first three decimal values.
// Truncates at the 3rd decimal place.
//
i32FPart[ui32Idx] = (int32_t) (pfData[ui32Idx] * 1000.0f);
//
// Subtract off the integer part from this newly formed decimal
// part.
//
i32FPart[ui32Idx] = i32FPart[ui32Idx]
- (i32IPart[ui32Idx] * 1000);
//
// make the decimal part a positive number for display.
//
if (i32FPart[ui32Idx] < 0) {
i32FPart[ui32Idx] *= -1;
}
}
if (g_bleUserFlag == 1) {
g_bleFlag = 0;
ble_cmd_attributes_write(58, 0, 12, (uint8_t*)pfEuler);
while (g_bleFlag == 0) {
}
} else if (g_bleDisconnectFlag == 1) {
ConfigureBLE();
}
//
// Print the acceleration numbers in the table.
//
// UARTprintf("%3d.%03d, ", i32IPart[0], i32FPart[0]);
// UARTprintf("%3d.%03d, ", i32IPart[1], i32FPart[1]);
// UARTprintf("%3d.%03d\n", i32IPart[2], i32FPart[2]);
//
// //
// // Print the angular velocities in the table.
// //
// UARTprintf("%3d.%03d, ", i32IPart[3], i32FPart[3]);
// UARTprintf("%3d.%03d, ", i32IPart[4], i32FPart[4]);
// UARTprintf("%3d.%03d\n", i32IPart[5], i32FPart[5]);
//
// //
// // Print the magnetic data in the table.
// //
// UARTprintf("%3d.%03d, ", i32IPart[6], i32FPart[6]);
// UARTprintf("%3d.%03d, ", i32IPart[7], i32FPart[7]);
// UARTprintf("%3d.%03d\n", i32IPart[8], i32FPart[8]);
//
// //
// // Print the direction in the table.
// //
// UARTprintf("%3d.%03d\n", i32IPart[16], i32FPart[16]);
// //
// // Print the Eulers in a table.
// //
// UARTprintf("%3d.%03d, ", i32IPart[9], i32FPart[9]);
// UARTprintf("%3d.%03d, ", i32IPart[10], i32FPart[10]);
// UARTprintf("%3d.%03d\n", i32IPart[11], i32FPart[11]);
//
// //
// // Print the quaternions in a table format.
// //
// UARTprintf("\033[19;14H%3d.%03d", i32IPart[12], i32FPart[12]);
// UARTprintf("\033[19;32H%3d.%03d", i32IPart[13], i32FPart[13]);
// UARTprintf("\033[19;50H%3d.%03d", i32IPart[14], i32FPart[14]);
// UARTprintf("\033[19;68H%3d.%03d", i32IPart[15], i32FPart[15]);
}
}
}
int
main(void)
{
char stringbuffer[17];
int distance = 0;
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
ROM_FPUEnable();
ROM_FPULazyStackingEnable();
ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART3);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_EEPROM0); //This wasn't clear at all. Note to self, everything needs enabling on this chip.
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_HIBERNATE);
ROM_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, WAKEPIN);
ROM_GPIOPinTypeGPIOInput(GPIO_PORTA_BASE, REEDPIN);
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, V5POWER);
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, V3POWER);
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, SERVO);
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1);
// ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2);
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_3);
ROM_GPIOPinWrite(GPIO_PORTE_BASE, V3POWER, 0xFF);
#ifdef EASYOPEN
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_0);
#endif
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
GPIOPinConfigure(GPIO_PC6_U3RX);
GPIOPinConfigure(GPIO_PC7_U3TX);
ROM_GPIOPinTypeUART(GPIO_PORTC_BASE, GPIO_PIN_6 | GPIO_PIN_7);
ROM_UARTConfigSetExpClk(UART0_BASE, ROM_SysCtlClockGet(), GPSBAUD,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
UART_CONFIG_PAR_NONE));
ROM_UARTConfigSetExpClk(UART3_BASE, ROM_SysCtlClockGet(), GPSBAUD,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
UART_CONFIG_PAR_NONE));
ROM_IntEnable(INT_UART0);
ROM_UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT);
ROM_IntEnable(INT_UART3);
ROM_UARTIntEnable(UART3_BASE, UART_INT_RX | UART_INT_RT);
SysTickPeriodSet(SysCtlClockGet()/10000);
SysTickIntRegister(&ServoDriver);
SysTickIntEnable();
SysTickEnable();
ROM_IntMasterEnable();
GPIOIntTypeSet(GPIO_PORTA_BASE, REEDPIN, GPIO_FALLING_EDGE);
GPIOPinIntClear(GPIO_PORTA_BASE, REEDPIN);
GPIOPinIntEnable(GPIO_PORTA_BASE, REEDPIN);
IntEnable(INT_GPIOA);
// while(1){}
/* SysCtlDelay(SysCtlClockGet()/1000);//Make sure the servo is going to get a pulse soon.
ROM_GPIOPinWrite(GPIO_PORTE_BASE, V5POWER, 0xFF); //Turn on the 5V power to LCD + servo.
SysCtlDelay(SysCtlClockGet()/1000);//Make sure the servo is going to get a pulse soon.*/
EEPROMInit();
initLCD();
LCDCommand(0x0c);
#ifdef LOOPBACKUART
while(1){}
#endif
#ifdef FIRSTRUN //First run, sets the eeprom to have as many tries as is desired.
EEPROMMassErase();
EEPROMProgram(&initialNumTries,eepromAddress,sizeof(initialNumTries));
LCDWriteText("Setup Complete. ", 0, 0);
LCDWriteText("Reflash Firmware", 1, 0);
while (1){} //Don't want to do anything else now.
#endif
EEPROMRead(&numTrieslong,eepromAddress,sizeof(numTrieslong));
// numTries=(int)numTrieslong;
// openLock();
// numTrieslong=0;
if (numTrieslong > initialNumTries-3) //Has already opened once, so just open as needed if stuck.
{
openLock();
numTrieslong--;
EEPROMProgram(&numTrieslong,eepromAddress,sizeof(numTrieslong)); //Decrement EEPROM counter.
}
else
{
distance = getDistance();
if(distance==99999){ //No fix :/
LCDWriteText("Location unknown", 0, 0);
LCDWriteText("Take me outside ", 1, 0);
SysCtlDelay(SysCtlClockGet()); //Waits 3 seconds.
}
else if (distance>NEARENOUGH) //Valid fix, too far away.
{
if ((int)numTrieslong>0) //Any attempts remaining?
{
usnprintf(stringbuffer,17,"Distance: %4dm ",distance);
LCDWriteText(stringbuffer, 0, 0);
numTrieslong--;
// numTries=(int)numTrieslong;
EEPROMProgram(&numTrieslong,eepromAddress,sizeof(numTrieslong)); //Decrement EEPROM counter.
usnprintf(stringbuffer,17,"%2d Attempts left",(int)numTrieslong);
LCDWriteText(stringbuffer, 1, 0);
SysCtlDelay(SysCtlClockGet()*2);
}
else
{
LCDWriteText("Oh dear... ", 0, 0); //Not really sure what to do, hopefully this code never runs.
LCDWriteText("Opening anyway. ", 1, 0);
// numTrieslong=initialNumTries+1;
// EEPROMProgram(&numTrieslong,eepromAddress,sizeof(initialNumTries)); //Set to big value
SysCtlDelay(10*SysCtlClockGet()/3);
openLock();
}
}
else //Found the location!
{
openLock();
numTrieslong=initialNumTries+1;
//numTries=(int)numTrieslong;
EEPROMProgram(&numTrieslong,eepromAddress,sizeof(initialNumTries)); //Lock will now open straight away.
}
}
// BLINK(RED);
HibernateEnableExpClk(SysCtlClockGet());
HibernateGPIORetentionEnable(); //Enables GPIO retention after wake from hibernate.
HibernateClockSelect(HIBERNATE_CLOCK_SEL_RAW);
HibernateWakeSet(HIBERNATE_WAKE_PIN);
HibernateIntRegister(&HibernateInterrupt);
HibernateIntEnable(HIBERNATE_INT_PIN_WAKE);
//BLINK(BLUE);
ROM_GPIOPinWrite(GPIO_PORTE_BASE, V5POWER, 0); //GPIO pins keep state on hibernate, so make sure to power everything else down.
ROM_GPIOPinWrite(GPIO_PORTE_BASE, V3POWER, 0); //GPIO pins keep state on hibernate, so make sure to power everything else down.
ROM_GPIOPinWrite(GPIO_PORTB_BASE, RS|E|D4|D5|D6|D7, 0xFF); //Pull all data pins to LCD high so we're not phantom powering it through ESD diodes.
ROM_GPIOPinWrite(GPIO_PORTF_BASE, SERVO, 0xFF); //Likewise for the servo
SysCtlDelay(SysCtlClockGet()/6);
HibernateRequest();// we want to be looping'n'shit.
while(1){} //Lalala, I'm a sleeping right now.
}
BOOL DR_VendorCmnd(void)
{
WORD addr, len, bc; // xdata used here to conserve data ram; if not EEPROM writes don't work anymore
WORD i;
// char *dscrRAM;
unsigned char xdata JTAGdata[400];
// we don't actually process the command here, we process it in the main loop
// here we just do the handshaking and ensure if it is a command that is implemented
switch (SETUPDAT[1]){
case VR_ENABLE_AE_IN: // enable IN transfers
{
startMonitor();
break; // handshake phase triggered below
}
case VR_DISABLE_AE_IN: // disable IN transfers
{
stopMonitor();
break;
}
case VR_RESET_FIFOS: // reset in and out fifo
{
SYNCDELAY;
EP6FIFOCFG = 0x01; //0000_0001 disable auto-in
SYNCDELAY;
FIFORESET = 0x80;
SYNCDELAY;
FIFORESET = 0x06;
SYNCDELAY;
FIFORESET = 0x00;
SYNCDELAY;
EP6FIFOCFG = 0x09 ; //0000_1001 reenable auto-in
break;
}
case VR_DOWNLOAD_CPLD_CODE:
{
if (SETUPDAT[0]==VR_DOWNLOAD) {
if (JTAGinit)
{
IOC=0x00;
OEC = 0xBD; // configure TDO (bit 6) and TSmaster as input : 1011_1101
xsvfInitialize();
JTAGinit=FALSE;
}
len = SETUPDAT[6];
len |= SETUPDAT[7] << 8;
if (len>400)
{
xsvfReturn=10;
OEC = 0x0D; // configure JTAG pins to float : 0000_1111
JTAGinit=TRUE;
break;
}
addr=0;
resetReadCounter(JTAGdata);
while(len) // Move new data through EP0OUT
{ // one packet at a time.
// Arm endpoint - do it here to clear (after sud avail)
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
while(EP0CS & bmEPBUSY);
bc = EP0BCL; // Get the new bytecount
for(i=0; i<bc; i++)
JTAGdata[addr+i] = EP0BUF[i];
addr += bc;
len -= bc;
}
if (SETUPDAT[2]==0x00) //complete
{
OEC = 0x0D; // configure JTAG pins to float : 0000_1111
JTAGinit=TRUE;
} else
{
xsvfReturn=xsvfRun();
if (xsvfReturn>0) // returns true if error
{
OEC = 0x0D; // configure JTAG pins to float : 0000_1101
JTAGinit=TRUE;
// return TRUE;
}
}
/* EP0BUF[0] = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
return(FALSE); */
break;
}
else //case VR_XSVF_ERROR_CODE:
{
EP0BUF[0] = SETUPDAT[1];
EP0BUF[1]= xsvfReturn;
EP0BCH = 0;
EP0BCL = 2;
EP0CS |= bmHSNAK;
return(FALSE);
}
}
/* case VR_SET_DEVICE_NAME:
{
*EP0BUF = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
while(EP0CS & bmEPBUSY); //wait for the data packet to arrive
dscrRAM = (char*)EZUSB_GetStringDscr(3); // get address of serial number descriptor-string in RAM
if (EP0BCL > MAX_NAME_LENGTH)
{
len=MAX_NAME_LENGTH;
} else
{
len=EP0BCL;
}
for (i=0;i<len;i++)
{
EEPROMWriteBYTE(STRING_ADDRESS+i, EP0BUF[i]); // write string to EEPROM
dscrRAM[2+i*2] = EP0BUF[i]; // write string to RAM
}
for (i=len; i<MAX_NAME_LENGTH; i++) // fill the rest with stop characters
{
EEPROMWriteBYTE(STRING_ADDRESS+i, ' '); // write string to EEPROM
dscrRAM[2+i*2] = ' '; // write string to RAM
}
EP0BCH = 0;
EP0BCL = 0;
return(FALSE);
}*/
case VR_RESETTIMESTAMPS:
{
RESET_TS=1; // assert RESET_TS pin for one instruction cycle (four clock cycles)
RESET_TS=0;
// reset dvs statemachines
IOE= IOE & ~DVS_nReset;
_nop_();
_nop_();
_nop_();
IOE = IOE | DVS_nReset; //start dvs statemachines
break;
}
case VR_WRITE_BIASGEN: // write bytes to SPI interface
case VR_EEPROM_BIASGEN_BYTES: // falls through and actual command is tested below
{
SYNCDELAY;
addr = SETUPDAT[2]; // Get address and length
addr |= SETUPDAT[3] << 8;
len = SETUPDAT[6];
len |= SETUPDAT[7] << 8;
numBiasBytes=len;
while(len){ // Move new data through EP0OUT, one packet at a time
// Arm endpoint - do it here to clear (after sud avail)
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
while(EP0CS & bmEPBUSY); // spin here until data arrives
bc = EP0BCL; // Get the new bytecount
// Is this a download to biasgen shift register?
if(SETUPDAT[1] == VR_WRITE_BIASGEN){
for(i=0; i<bc; i++){
spiwritebyte(EP0BUF[i]);
biasBytes[i]=EP0BUF[i]; // not sure why these are saved...
}
}else{ // we write EEProm starting at addr with bc bytes from EP0BUF
// EEPROMWrite(addr,bc,(WORD)EP0BUF);
}
addr += bc; // inc eeprom addr to write to, in case that's what we're doing
len -= bc; // dec total byte count
}
if(SETUPDAT[1]==VR_WRITE_BIASGEN) {
latchNewBiases();
//setLatchTransparent(); // let values pass through latch from shift register -- these are new values
//setLatchOpaque();
}
EP0BCH = 0;
EP0BCL = 0; // Arm endpoint with 0 byte to transfer
LED=!LED;
return(FALSE); // very important, otherwise get stall
}
case VR_SET_POWERDOWN: // control powerDown output bit
{
if (SETUPDAT[2])
{
setPowerDownBit();
} else
{
releasePowerDownBit();
}
*EP0BUF=VR_SET_POWERDOWN;
SYNCDELAY;
EP0BCH = 0;
EP0BCL = 1; // Arm endpoint with 1 byte to transfer
EP0CS |= bmHSNAK; // Acknowledge handshake phase of device request
break; // very important, otherwise get stall
}
/* TCVS320 doesn't have global array reset */
/*
case VR_SETARRAYRESET: // set array reset, based on lsb of argument
{
if (SETUPDAT[2]&0x01)
{
IOE|=arrayReset;
} else
{
IOE&=~arrayReset;
}
*EP0BUF=VR_SETARRAYRESET;
SYNCDELAY;
EP0BCH = 0;
EP0BCL = 1; // Arm endpoint with 1 byte to transfer
EP0CS |= bmHSNAK; // Acknowledge handshake phase of device request
return(FALSE); // very important, otherwise get stall
}
case VR_DOARRAYRESET: // reset array for fixed reset time
{
IOE&=~arrayReset;
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_(); // a few us
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
IOE|=arrayReset;
*EP0BUF=VR_DOARRAYRESET;
SYNCDELAY;
EP0BCH = 0;
EP0BCL = 1; // Arm endpoint with 1 byte to transfer
EP0CS |= bmHSNAK; // Acknowledge handshake phase of device request
return (FALSE); // very important, otherwise get stall
}
*/
/* case VR_DOWNLOAD_CPLD_CODE:
{
break;
} */
case VR_IS_TS_MASTER:
{
EP0BUF[0] = SETUPDAT[1];
EP0BUF[1]= TIMESTAMP_MASTER;
EP0BCH = 0;
EP0BCL = 2;
EP0CS |= bmHSNAK;
return(FALSE);
}
case VR_RAM:
case VR_EEPROM:
{
addr = SETUPDAT[2]; // Get address and length
addr |= SETUPDAT[3] << 8;
len = SETUPDAT[6];
len |= SETUPDAT[7] << 8;
// Is this an upload command ?
if(SETUPDAT[0] == VR_UPLOAD) // this is automatically defined on host from direction of vendor request
{
while(len) // Move requested data through EP0IN
{ // one packet at a time.
while(EP0CS & bmEPBUSY);
if(len < EP0BUFF_SIZE)
bc = len;
else
bc = EP0BUFF_SIZE;
// Is this a RAM upload ?
if(SETUPDAT[1] == VR_RAM)
{
for(i=0; i<bc; i++)
*(EP0BUF+i) = *((BYTE xdata *)addr+i);
}
else
{
for(i=0; i<bc; i++)
*(EP0BUF+i) = 0xcd;
EEPROMRead(addr,(WORD)bc,(WORD)EP0BUF);
}
EP0BCH = 0;
EP0BCL = (BYTE)bc; // Arm endpoint with # bytes to transfer
addr += bc;
len -= bc;
}
}
// Is this a download command ?
else if(SETUPDAT[0] == VR_DOWNLOAD) // this is automatically defined on host from direction of vendor request
{
while(len) // Move new data through EP0OUT
{ // one packet at a time.
// Arm endpoint - do it here to clear (after sud avail)
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
while(EP0CS & bmEPBUSY);
bc = EP0BCL; // Get the new bytecount
// Is this a RAM download ?
if(SETUPDAT[1] == VR_RAM)
{
for(i=0; i<bc; i++)
*((BYTE xdata *)addr+i) = *(EP0BUF+i);
}
else
EEPROMWrite(addr,bc,(WORD)EP0BUF);
addr += bc;
len -= bc;
}
}
return(FALSE);
}
default:
{ // we received an invalid command
return(TRUE);
}
}
*EP0BUF = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
return(FALSE);
}
void main(void)
{
#define BAUDRG 77
BYTE SecNum = 0;
BOOL Tx_Success = FALSE;
BYTE Tx_Trials = 0, scanresult = 0;
/*******************************************************************/
// Initialize the system
/*******************************************************************/
ANCON0 = 0XFF; /*desactiva entradas analogicas*/
ANCON1 = 0XFF; /*desactiva entradas analogicas*/
PPSUnLock();
PPSOutput(PPS_RP10, PPS_TX2CK2); // TX2 RP17/RC6 icsp
PPSInput(PPS_RX2DT2, PPS_RP9); // RX2 RP18/RC7
PPSOutput(PPS_RP23, PPS_SDO2); // SDO2 RP23/RD6
PPSInput(PPS_SDI2, PPS_RP24); // SDI2 RP24/RD7
PPSOutput(PPS_RP22, PPS_SCK2); // SCK2 RP22/RD5
PPSLock();
System_PeripheralPinSelect( ExternalInterrupt3, 19); /*external interrupt 3 B3*/
BoardInit();
ConsoleInit();
Gpios_PinDirection(GPIOS_PORTC, 7, GPIOS_INPUT); /*pin C0 como salida para SDI*/
Gpios_PinDirection(GPIOS_PORTC, 6, GPIOS_OUTPUT); /*pin C1 como salida para SDO*/
//Gpios_PinDirection(GPIOS_PORTD, 4, GPIOS_OUTPUT); /*pin D4 como salida */
Open1USART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_EIGHT_BIT & USART_ASYNCH_MODE & USART_ADDEN_OFF, BAUDRG);
baud1USART(BAUD_IDLE_TX_PIN_STATE_HIGH & BAUD_IDLE_RX_PIN_STATE_HIGH & BAUD_AUTO_OFF & BAUD_WAKEUP_OFF & BAUD_16_BIT_RATE & USART_RX_INT_OFF);
Open2USART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_EIGHT_BIT & USART_ASYNCH_MODE & USART_ADDEN_OFF, BAUDRG);
baud2USART(BAUD_IDLE_TX_PIN_STATE_HIGH & BAUD_IDLE_RX_PIN_STATE_HIGH & BAUD_AUTO_OFF & BAUD_WAKEUP_OFF & BAUD_16_BIT_RATE & USART_RX_INT_OFF);
OpenTimer1( TIMER_INT_OFF &T1_16BIT_RW &T1_SOURCE_FOSC_4 & T1_PS_1_8 &T1_OSC1EN_OFF &T1_SYNC_EXT_OFF, TIMER_GATE_OFF & TIMER_GATE_INT_OFF);
Gpios_PinDirection(GPIOS_PORTD, 7, GPIOS_INPUT); /*pin C0 como salida para SDI*/
Gpios_PinDirection(GPIOS_PORTD, 6, GPIOS_OUTPUT); /*pin C1 como salida para SDO*/
Gpios_PinDirection(GPIOS_PORTD, 5, GPIOS_OUTPUT); /*pin C2 como salida para SCK*/
Spi_Init(SPI_PORT1, SPI_64DIV); /*Inicializamos SPI2*/
Spi_Init(SPI_PORT2, SPI_64DIV); /*Inicializamos SPI2*/
//Adc_Init(ADC_10BITS);
LED_1 = 1;
LED_2 = 0;
//RLY_1 = 0;
RLY_2 = 0;
ON_RADIO = 1;
ON_MAC = 1;
ON_TEMP = 1;
StartWirelessConnection();
//myDevicesRequiredStatus[0] = 0x55;
EEPROMRead(&myDevicesRequiredStatus, 0, 1);
if(myDevicesRequiredStatus[0] == 0x55)
{
RLY_1 = 1;
EEPROMCHG = 1;
ConsolePutROMString((ROM char *)"RELAY ON ");
}
if(myDevicesRequiredStatus[0] == 0xAA)
{
RLY_1 = 0;
EEPROMCHG = 0;
ConsolePutROMString((ROM char *)"RELAY OFF ");
}
for(j=0;j<10;j++)
{
DelayMs(50);
LED_1 ^= 1;
LED_2 ^= 1;
}
LED_1 = 0;
LED_2 = 0;
//RLY_1 = 0;
RLY_2 = 0;
TickScaler = 4;
EndDevStateMachine =0;
/*
while(!Tx_Success)
{
if(myChannel < 8)
scanresult = MiApp_SearchConnection(10, (0x00000001 << myChannel));
else if(myChannel < 16)
scanresult = MiApp_SearchConnection(10, (0x00000100 << (myChannel-8)));
else if(myChannel < 24)
scanresult = MiApp_SearchConnection(10, (0x00010000 << (myChannel-16)));
else
scanresult = MiApp_SearchConnection(10, (0x01000000 << (myChannel-24)));
if(scanresult == 0)
{
Tx_Trials++;
if(Tx_Trials > 2) break;
}
else Tx_Success = TRUE;
}
if(Tx_Success)
{
ConsolePutROMString((ROM char *)"RADIO OK ");
}
else
{
ConsolePutROMString((ROM char *)"RADIO FAIL ");
}
*/
//.VBGOE = 0;
//ANCON1bits.VBGEN = 1; // Enable Band gap reference voltage
//DelayMs(10);
//VBGResult = Adc_u16Read(15);
//ANCON1bits.VBGEN = 0; // Disable Bandgap
//Adc_Init(ADC_10BITS);
ANCON0 = 0xFF;
ANCON1 = 0x9F;
ANCON1bits.VBGEN = 1; // Enable Band gap reference voltage
ADCON0bits.VCFG = 0; // vreff VDD-VSS
ADCON0bits.CHS = 0x0F; // VBG channel select
ADCON1 = 0xBE;
ADCON0bits.ADON = 1;
//for(j=0;j<16;j++)
//{
//myDevicesOutputStatus[j] = j;
//}
//EEPROMWRITE(myDevicesOutputStatus,0,16);
//for(j=0;j<16;j++)
//{
//myDevicesOutputStatus[j] = 0;
//}
//DelayMs(500);
EEPROMRead(&myDevicesOutputStatus, 0, 16);
ConsolePutROMString((ROM char *)"EEPROM READ: ");
//PrintChar(TemperatureCalibrationValue);
for(j=0;j<1;j++)
{
PrintChar(myDevicesOutputStatus[j]);
}
SwTimer3 = 0;
SwTimer4 = 0;
TRISB&=0xEF; //JL: Configuro el pin B4 como salida si modificar el estado de los demas pines
while(1)
{
/*
WirelessTxRx();
WirelesStatus();
Bypass();
*/
//No se utilizaron
//Menu();
//Timer1Tick();
//WirelessTxRxPANCOORD();
//TaskScheduler();
//JLEstas funciones deben habilitarse para trabajar como repetidora
Timer1Tick();
Repeater();
}
}
int lasat_init_board_info(void)
{
int c;
unsigned long crc;
unsigned long cfg0, cfg1;
const struct product_info *ppi;
int i_n_base_models = N_BASE_MODELS;
const char * const * i_txt_base_models = txt_base_models;
int i_n_prids = N_PRIDS;
memset(&lasat_board_info, 0, sizeof(lasat_board_info));
/* First read the EEPROM info */
EEPROMRead(0, (unsigned char *)&lasat_board_info.li_eeprom_info,
sizeof(struct lasat_eeprom_struct));
/* Check the CRC */
crc = EEPROM_CRC((unsigned char *)(&lasat_board_info.li_eeprom_info),
sizeof(struct lasat_eeprom_struct) - 4);
if (crc != lasat_board_info.li_eeprom_info.crc32) {
printk(KERN_WARNING "WARNING...\nWARNING...\nEEPROM CRC does "
"not match calculated, attempting to soldier on...\n");
}
if (lasat_board_info.li_eeprom_info.version != LASAT_EEPROM_VERSION) {
printk(KERN_WARNING "WARNING...\nWARNING...\nEEPROM version "
"%d, wanted version %d, attempting to soldier on...\n",
(unsigned int)lasat_board_info.li_eeprom_info.version,
LASAT_EEPROM_VERSION);
}
cfg0 = lasat_board_info.li_eeprom_info.cfg[0];
cfg1 = lasat_board_info.li_eeprom_info.cfg[1];
if (LASAT_W0_DSCTYPE(cfg0) != 1) {
printk(KERN_WARNING "WARNING...\nWARNING...\n"
"Invalid configuration read from EEPROM, attempting to "
"soldier on...");
}
/* We have a valid configuration */
switch (LASAT_W0_SDRAMBANKSZ(cfg0)) {
case 0:
lasat_board_info.li_memsize = 0x0800000;
break;
case 1:
lasat_board_info.li_memsize = 0x1000000;
break;
case 2:
lasat_board_info.li_memsize = 0x2000000;
break;
case 3:
lasat_board_info.li_memsize = 0x4000000;
break;
case 4:
lasat_board_info.li_memsize = 0x8000000;
break;
default:
lasat_board_info.li_memsize = 0;
}
switch (LASAT_W0_SDRAMBANKS(cfg0)) {
case 0:
break;
case 1:
lasat_board_info.li_memsize *= 2;
break;
default:
break;
}
switch (LASAT_W0_BUSSPEED(cfg0)) {
case 0x0:
lasat_board_info.li_bus_hz = 60000000;
break;
case 0x1:
lasat_board_info.li_bus_hz = 66000000;
break;
case 0x2:
lasat_board_info.li_bus_hz = 66666667;
break;
case 0x3:
lasat_board_info.li_bus_hz = 80000000;
break;
case 0x4:
lasat_board_info.li_bus_hz = 83333333;
break;
case 0x5:
lasat_board_info.li_bus_hz = 100000000;
break;
}
switch (LASAT_W0_CPUCLK(cfg0)) {
case 0x0:
lasat_board_info.li_cpu_hz =
lasat_board_info.li_bus_hz;
break;
case 0x1:
lasat_board_info.li_cpu_hz =
lasat_board_info.li_bus_hz +
(lasat_board_info.li_bus_hz >> 1);
break;
case 0x2:
lasat_board_info.li_cpu_hz =
lasat_board_info.li_bus_hz +
lasat_board_info.li_bus_hz;
break;
case 0x3:
lasat_board_info.li_cpu_hz =
lasat_board_info.li_bus_hz +
lasat_board_info.li_bus_hz +
(lasat_board_info.li_bus_hz >> 1);
break;
case 0x4:
lasat_board_info.li_cpu_hz =
lasat_board_info.li_bus_hz +
lasat_board_info.li_bus_hz +
lasat_board_info.li_bus_hz;
break;
}
/* Flash size */
switch (LASAT_W1_FLASHSIZE(cfg1)) {
case 0:
lasat_board_info.li_flash_size = 0x200000;
break;
case 1:
lasat_board_info.li_flash_size = 0x400000;
break;
case 2:
lasat_board_info.li_flash_size = 0x800000;
break;
case 3:
lasat_board_info.li_flash_size = 0x1000000;
break;
case 4:
lasat_board_info.li_flash_size = 0x2000000;
break;
}
init_flash_sizes();
lasat_board_info.li_bmid = LASAT_W0_BMID(cfg0);
lasat_board_info.li_prid = lasat_board_info.li_eeprom_info.prid;
if (lasat_board_info.li_prid == 0xffff || lasat_board_info.li_prid == 0)
lasat_board_info.li_prid = lasat_board_info.li_bmid;
/* Base model stuff */
if (lasat_board_info.li_bmid > i_n_base_models)
lasat_board_info.li_bmid = i_n_base_models;
strcpy(lasat_board_info.li_bmstr,
i_txt_base_models[lasat_board_info.li_bmid]);
/* Product ID dependent values */
c = lasat_board_info.li_prid;
if (c >= i_n_prids) {
strcpy(lasat_board_info.li_namestr, "Unknown Model");
strcpy(lasat_board_info.li_typestr, "Unknown Type");
} else {
ppi = &vendor_info_table[0].vi_product_info[c];
strcpy(lasat_board_info.li_namestr, ppi->pi_name);
if (ppi->pi_type)
strcpy(lasat_board_info.li_typestr, ppi->pi_type);
else
sprintf(lasat_board_info.li_typestr, "%d", 10 * c);
}
return 0;
}
/*******************************************************************************
* Pull EP1 data
*******************************************************************************/
void ep1_pool(void){
BYTE i;
WORD adr;
BYTE new_data = 0;
// Test data for internal test
if(FPGA_INT0 && FPGA_DONE && !prev_done && !cmd_cnt){
EP8FIFOCFG = 0x00; SYNCDELAY;
FIFORESET = 0x08; SYNCDELAY;
FIFORESET = 0x00; SYNCDELAY;
EP8FIFOBUF[0] = 0x12;
EP8FIFOBUF[1] = 0x34;
EP8FIFOBUF[2] = 0x56;
EP8FIFOBUF[3] = 0x78;
EP8FIFOBUF[4] = 0x90;
EP8FIFOBUF[5] = 0xAB;
EP8FIFOBUF[6] = 0xCD;
EP8FIFOBUF[7] = 0xEF;
EP8BCH = 0;
EP8BCL = 8;
EP8FIFOCFG = 0x10; SYNCDELAY;
prev_done = 1;
}
if( !( EP1OUTCS & 0x02) ){ // Got something
cmd_cnt++;
for (i = 0; i < 0x40; i++)
EP1INBUF[i] = 0xFF; // fill output buffer
switch(EP1OUTBUF[0]){ // Decode command
//-----------------------------------------------------------------
default:
case CMD_READ_VERSION:
EP1INBUF[0] = fx2_ver_maj_;
EP1INBUF[1] = fx2_ver_min_;
EP1INBUF[2] = fx2_tip_maj_;
EP1INBUF[3] = fx2_tip_min_;
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_SET_AUTORESPONSE:
sts_int_auto_configured = 1;
iar_adress = EP1OUTBUF[1];
iar_count = EP1OUTBUF[2];
iar_int_idx = 0;
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_GET_AUTORESPONSE:
EP1INBUF[0] = iar_int_idx;
for(i = 0; i < 32; i++)
EP1INBUF[i+1] = auto_response_data[i];
iar_int_idx = 0;
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_SWITCH_MODE:
sts_current_mode = 1;
new_data = 1;
EP1INBUF[0] = EP1OUTBUF[1];
break;
//-----------------------------------------------------------------
case CMD_READ_STATUS:
sts_flash_busy = get_flash_busy();
sts_booting = FPGA_DONE;
sts_fpga_prog = 0xaa;
sts_high_speed_mode = (USBCS & bmHSM) ? 1 : 255;
new_data = 1;
EP1INBUF[0] = sts_fifo_error;
EP1INBUF[1] = sts_current_mode;
EP1INBUF[2] = sts_flash_busy;
EP1INBUF[3] = sts_fpga_prog;
EP1INBUF[4] = sts_booting;
EP1INBUF[5] = sts_i2c_new_data;
EP1INBUF[6] = sts_int_auto_configured;
EP1INBUF[7] = sts_high_speed_mode;
sts_i2c_new_data = 0;
break;
//-----------------------------------------------------------------
case CMD_RESET_FIFO_STATUS:
sts_fifo_error = 0;
FIFORESET = 0x80; SYNCDELAY; // NAK all requests from host.
switch(EP1OUTBUF[1]){
case 2:
EP2FIFOCFG = 0x48; SYNCDELAY;
FIFORESET = 0x02; SYNCDELAY;
break;
case 4:
EP4FIFOCFG = 0x48; SYNCDELAY;
FIFORESET = 0x04; SYNCDELAY;
break;
case 6:
EP6FIFOCFG = 0x48; SYNCDELAY;
FIFORESET = 0x06; SYNCDELAY;
break;
default: // 0
EP2FIFOCFG = 0x48; SYNCDELAY;
EP4FIFOCFG = 0x48; SYNCDELAY;
EP6FIFOCFG = 0x48; SYNCDELAY;
EP8FIFOCFG = 0x10; SYNCDELAY;
FIFORESET = 0x02; SYNCDELAY;
FIFORESET = 0x04; SYNCDELAY;
FIFORESET = 0x06; SYNCDELAY;
}
FIFORESET = 0x00; SYNCDELAY; // Resume normal operation.
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_FLASH_WRITE:
if (EP1OUTBUF[4] > 59) EP1OUTBUF[4] = 59;
page_write(EP1OUTBUF[1], EP1OUTBUF[2], EP1OUTBUF[3], &EP1OUTBUF[5], EP1OUTBUF[4]); //highest, high, low adr, read_ptr, size
//-----------------------------------------------------------------
case CMD_FLASH_READ:
if (EP1OUTBUF[4] > 64) EP1OUTBUF[4] = 64;
page_read(EP1OUTBUF[1], EP1OUTBUF[2], EP1OUTBUF[3], &EP1INBUF[0], EP1OUTBUF[4]); //highest, high, low adr, read_ptr, size
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_FLASH_ERASE:
// busy_polling();
// On some modules it cause API error - better to do it from software side
bulk_erase();
new_data = 1;
sts_flash_busy = 1;
break;
//-----------------------------------------------------------------
case CMD_SECTOR_ERASE:
sector_erase(EP1OUTBUF[1]);
new_data = 1;
sts_flash_busy = 1;
break;
//-----------------------------------------------------------------
case CMD_FLASH_WRITE_COMMAND:
EP1INBUF[0] = 0x55;
spi_command(EP1OUTBUF[1], &EP1OUTBUF[3], EP1OUTBUF[2], &EP1INBUF[1]);
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_EEPROM_WRITE:
adr = EP1OUTBUF[1];
adr = (adr << 8) + EP1OUTBUF[2];
if (EP1OUTBUF[3] > 32) EP1OUTBUF[3] = 32;
EEPROMWrite(adr, EP1OUTBUF[3], &EP1OUTBUF[4]); // adress, size, data
//-----------------------------------------------------------------
case CMD_EEPROM_READ:
adr = EP1OUTBUF[1];
adr = (adr << 8) + EP1OUTBUF[2];
EEPROMRead(adr, EP1OUTBUF[3], &EP1INBUF[0]); // adress, size, data
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_GET_FIFO_STATUS:
EP1INBUF[0] = EP2CS;
EP1INBUF[1] = EP4CS;
EP1INBUF[2] = EP6CS;
EP1INBUF[3] = EP8CS;
EP1INBUF[4] = EP2FIFOBCH;
EP1INBUF[5] = EP4FIFOBCH;
EP1INBUF[6] = EP6FIFOBCH;
EP1INBUF[7] = EP8FIFOBCH;
EP1INBUF[8] = EP2FIFOBCL;
EP1INBUF[9] = EP4FIFOBCL;
EP1INBUF[10] = EP6FIFOBCL;
EP1INBUF[11] = EP8FIFOBCL;
EP1INBUF[12] = EP2FIFOFLGS;
EP1INBUF[13] = EP4FIFOFLGS;
EP1INBUF[14] = EP6FIFOFLGS;
EP1INBUF[15] = EP8FIFOFLGS;
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_I2C_WRITE:
I2CWrite(EP1OUTBUF[1], EP1OUTBUF[2], &EP1OUTBUF[3]); // adress, size, data
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_I2C_READ:
I2CRead(EP1OUTBUF[1], EP1OUTBUF[2], &EP1INBUF[0]); // adress, size, data
new_data = 1;
break;
//-----------------------------------------------------------------
/*
case CMD_I2C_WRITE_READ:
i = EP1OUTBUF[1];
I2CWrite(i, EP1OUTBUF[2], &EP1OUTBUF[4]); // adress, size, data
delaycnt = 0;
while (INT0_PIN == 0){
EZUSB_Delay1ms();
delaycnt++;
if (delaycnt > 800)
break;
continue;
}
I2CRead(i, EP1OUTBUF[3], &EP1INBUF[0]); // adress, size, data
new_data = 1;
break;
*/
//-----------------------------------------------------------------
case CMD_FPGA_POWER:
if (EP1OUTBUF[1] == 0){
FPGA_POWER = 0;
sts_int_auto_configured = 0;
}
else{
IOD = 0x03; // Enable Power and disable Reset
OED = 0x03; // PROG_B and POWER
FPGA_POWER = 1;
}
EP1INBUF[0] = (FPGA_POWER) ? 1 : 0;
EP1INBUF[1] = 0xAA;
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_FPGA_RESET:
FPGA_INT1 = (EP1OUTBUF[1]) ? 1 : 0;
EP1INBUF[0] = FPGA_INT1;
EP1INBUF[1] = 0xAA;
new_data = 1;
break;
//-----------------------------------------------------------------
case CMD_DEV_LOCK:
if(EP1OUTBUF[1] == 0x01){ // Driver trying to lock device
if(lock == 0){ // Device is free
EP1INBUF[0] = 0x22; // Sucessfull lock
lock = 1;
}
else // Device is locked
EP1INBUF[0] = 0x00; // Already locked
}
else{ // Driver trying to unlock device
if(lock == 1){ // Device is locked
EP1INBUF[0] = 0x33; // Sucessfull unlock
lock = 0;
}
else // Device is unlocked
EP1INBUF[0] = 0x00; // Got problem
}
new_data = 1;
break;
//-----------------------------------------------------------------
}
EP1OUTBC = EP1DATA_COUNT; // Free input buffer
}
if(new_data){ // Have something to send
if ( !(EP1INCS & 0x02)){ // Can send ?
EP1INBC = EP1DATA_COUNT; // Send
new_data = 0;
}
}
}
CPSINPEX
#undef INP
#define INP(nnnn) static UINT8 Inp##nnnn;
CPSINPEX
#undef INP
// Read input port 0x000-0x1ff
static UINT8 CpsReadPort(const UINT32 ia)
{
UINT8 d = 0xFF;
if (ia == 0x000) {
d = (UINT8)~Inp000;
if (Pzloop2) {
if (ReadPaddle) {
d -= CpsPaddle2Value;
} else {
d = CpsPaddle2;
}
}
return d;
}
if (ia == 0x001) {
d = (UINT8)~Inp001;
if (Pzloop2) {
if (ReadPaddle) {
d -= CpsPaddle1Value;
} else {
d = CpsPaddle1;
}
}
return d;
}
if (ia == 0x010) {
d = (UINT8)~Inp010;
return d;
}
if (ia == 0x011) {
d = (UINT8)~Inp011;
return d;
}
if (ia == 0x012) {
d = (UINT8)~Inp012;
return d;
}
if (ia == 0x018) {
d = (UINT8)~Inp018;
return d;
}
if (ia == 0x019) {
d = (UINT8)~Inp019;
return d;
}
if (ia == 0x01B) {
d = (UINT8)~Inp01B;
return d;
}
if (ia == 0x01A) {
d = (UINT8)~Cpi01A;
return d;
}
if (ia == 0x01C) {
d = (UINT8)~Cpi01C;
return d;
}
if (ia == 0x01E) {
d = (UINT8)~Cpi01E;
return d;
}
if (Cps == 2) {
// Used on CPS2 only I think
if (ia == 0x020) {
d = (UINT8)~Inp020;
return d;
}
if (ia == 0x021) {
d = (UINT8)~Inp021;
d &= 0xFE;
d |= EEPROMRead();
return d;
}
// CPS2 Volume control
if (ia == 0x030) {
if (Ssf2tb) {
d = 0x20;
} else {
d = Cps2VolumeStates[Cps2Volume] >> 8;
if (Cps2DisableDigitalVolume) d = 0xd0;
}
return d;
}
if (ia == 0x031) {
d = Cps2VolumeStates[Cps2Volume] & 0xff;
return d;
}
if (ia >= 0x0100 && ia < 0x0200) {
static INT32 nRasterLine;
// bprintf(PRINT_NORMAL, _T(" - port 0x%02X (%3i)\n"), ia & 255, SekCurrentScanline());
// The linecounters seem to return the line at which the last IRQ triggered by this counter is scheduled minus the current line
if ((ia & 0x0FE) == 0x50) {
if ((ia & 1) == 0) {
nRasterLine = nIrqLine50 - SekCurrentScanline();
return nRasterLine >> 8;
} else {
return nRasterLine & 0xFF;
}
}
if ((ia & 0x0FE) == 0x52) {
if ((ia & 1) == 0) {
nRasterLine = nIrqLine52 - SekCurrentScanline();
return nRasterLine >> 8;
} else {
return nRasterLine & 0xFF;
void CheckESCFlag(){
int16_u outInt16;
uint32_t timeOutTimer = 0;
uint8_t LEDPatternArray[4];
if (EEPROMRead(PWM_FLAG) != 0xAA){
pwmHigh = PWM_HIGH_MAX;
pwmLow = PWM_LOW_MIN;
outInt16.val = pwmHigh;
EEPROMWrite(PWM_LIM_HIGH_START,outInt16.buffer[0]);
EEPROMWrite(PWM_LIM_HIGH_END,outInt16.buffer[1]);
outInt16.val = pwmLow;
EEPROMWrite(PWM_LIM_LOW_START,outInt16.buffer[0]);
EEPROMWrite(PWM_LIM_LOW_END,outInt16.buffer[1]);
EEPROMWrite(PWM_FLAG,0xAA);
}
outInt16.buffer[0] = EEPROMRead(PWM_LIM_HIGH_START);
outInt16.buffer[1] = EEPROMRead(PWM_LIM_HIGH_END);
pwmHigh = outInt16.val;
if (pwmHigh > PWM_HIGH_MAX){
pwmHigh = PWM_HIGH_MAX;
}
if (pwmHigh < PWM_HIGH_MIN){
pwmHigh = PWM_HIGH_MIN;
}
outInt16.buffer[0] = EEPROMRead(PWM_LIM_LOW_START);
outInt16.buffer[1] = EEPROMRead(PWM_LIM_LOW_END);
pwmLow = outInt16.val;
if (pwmLow < PWM_LOW_MIN){
pwmLow = PWM_LOW_MIN;
}
if (pwmLow > PWM_LOW_MAX){
pwmLow = PWM_LOW_MAX;
}
timeOutTimer = millis();
if (EEPROMRead(ESC_CAL_FLAG) == 0xAA){
calibrationMode = false;
if (rcDetected == false){
LEDPatternSet(1,1,0,1);
while(1){
}
}
else{
newRC = false;
while(newRC == false){
}
ProcessChannels();
newRC = false;
}
while(RCValue[THRO] > 1100 || RCValue[AILE] > 1100 || RCValue[ELEV] > 1100 || RCValue[RUDD] > 1100){
if (millis() - timeOutTimer > 60000){
EEPROMWrite(ESC_CAL_FLAG,0xFF);
LEDPatternSet(3,3,3,1);
while(1){
}
}
if (newRC == true){
newRC = false;
ProcessChannels();
if (RCValue[THRO] > 1100 ){
LEDPatternArray[0] = 5;
}
else{
LEDPatternArray[0] = 1;
}
if (RCValue[AILE] > 1100 ){
LEDPatternArray[1] = 5;
}
else{
LEDPatternArray[1] = 1;
}
if (RCValue[ELEV] > 1100 ){
LEDPatternArray[2] = 5;
}
else{
LEDPatternArray[2] = 1;
}
if (RCValue[RUDD] > 1100 ){
LEDPatternArray[3] = 5;
}
else{
LEDPatternArray[3] = 1;
}
LEDPatternSet(LEDPatternArray[0],LEDPatternArray[1],LEDPatternArray[2],LEDPatternArray[3]);
}
}
CompleteESCCalibration();
}
if (EEPROMRead(ESC_CAL_FLAG) == 0xBB){
calibrationMode = false;
calibrateESCs = false;
TryHandShake();
if (handShake == false || calibrationModeESCs == false){
EEPROMWrite(ESC_CAL_FLAG,0xFF);
LEDPatternSet(4,4,4,1);
while(1){
}
}
while(calibrateESCs == false){
if (millis() - timeOutTimer > 60000){
EEPROMWrite(ESC_CAL_FLAG,0xFF);
LEDPatternSet(3,3,3,1);
while(1){
}
}
LEDPatternSet(6,1,1,1);
Radio();
}
if (calibrateESCs == true){
CompleteESCCalibration();
}
}
}
BOOL DR_VendorCmnd(void)
{
WORD value;
WORD len,ind, bc; // xdata used here to conserve data ram; if not EEPROM writes don't work anymore
/*
union {
unsigned short ushort;
unsigned msb,lsb;
unsigned bytes[2]; // big endian, bytes[0] is MSB as far as C51 is concerned
} length;
*/
WORD i;
char *dscrRAM;
unsigned char xdata JTAGdata[400];
switch (SETUPDAT[1]){
case VR_ENABLE_AE_IN: // enable IN transfers
{
startMonitor();
break; // handshake phase triggered below
}
case VR_DISABLE_AE_IN: // disable IN transfers
{
stopMonitor();
break;
}
case VR_RESET_FIFOS: // reset in and out fifo
{
SYNCDELAY;
EP6FIFOCFG = 0x00; //0000_0000 disable auto-in
SYNCDELAY;
FIFORESET = 0x80;
SYNCDELAY;
FIFORESET = 0x06;
SYNCDELAY;
FIFORESET = 0x00;
SYNCDELAY;
EP6FIFOCFG = 0x08 ; //0000_1000 reenable auto-in
break;
}
case VR_DOWNLOAD_CPLD_CODE:
{
if (SETUPDAT[0]==VR_DOWNLOAD) {
if (JTAGinit)
{
IOC=0x00;
OEC = 0xBD; // configure TDO (bit 6) and TSmaster as input : 1011_1101
xsvfInitialize();
JTAGinit=FALSE;
}
len = SETUPDAT[6];
len |= SETUPDAT[7] << 8;
if (len>400)
{
xsvfReturn=10;
OEC = 0x0D; // configure JTAG pins to float : 0000_1111
JTAGinit=TRUE;
break;
}
value=0;
resetReadCounter(JTAGdata);
while(len) // Move new data through EP0OUT
{ // one packet at a time.
// Arm endpoint - do it here to clear (after sud avail)
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
while(EP0CS & bmEPBUSY);
bc = EP0BCL; // Get the new bytecount
for(i=0; i<bc; i++)
JTAGdata[value+i] = EP0BUF[i];
value += bc;
len -= bc;
}
if (SETUPDAT[2]==0x00) //complete
{
OEC = 0x0D; // configure JTAG pins to float : 0000_1111
JTAGinit=TRUE;
} else
{
xsvfReturn=xsvfRun();
if (xsvfReturn>0) // returns true if error
{
OEC = 0x0D; // configure JTAG pins to float : 0000_1101
JTAGinit=TRUE;
// return TRUE;
}
}
/* EP0BUF[0] = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
return(FALSE); */
break;
}
else //case VR_XSVF_ERROR_CODE:
{
EP0BUF[0] = SETUPDAT[1];
EP0BUF[1]= xsvfReturn;
EP0BCH = 0;
EP0BCL = 2;
EP0CS |= bmHSNAK;
return(FALSE);
}
}
case VR_SET_DEVICE_NAME:
{
*EP0BUF = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
while(EP0CS & bmEPBUSY); //wait for the data packet to arrive
dscrRAM = (char*)EZUSB_GetStringDscr(3); // get address of serial number descriptor-string in RAM
if (EP0BCL > MAX_NAME_LENGTH)
{
len=MAX_NAME_LENGTH;
} else
{
len=EP0BCL;
}
for (i=0;i<len;i++)
{
EEPROMWriteBYTE(STRING_ADDRESS+i, EP0BUF[i]); // write string to EEPROM
dscrRAM[2+i*2] = EP0BUF[i]; // write string to RAM
}
for (i=len; i<MAX_NAME_LENGTH; i++) // fill the rest with stop characters
{
EEPROMWriteBYTE(STRING_ADDRESS+i, ' '); // write string to EEPROM
dscrRAM[2+i*2] = ' '; // write string to RAM
}
EP0BCH = 0;
EP0BCL = 0;
return(FALSE);
}
case VR_RESETTIMESTAMPS:
{
tsReset=1; // RESET_TS=1; // assert RESET_TS pin for one instruction cycle (four clock cycles)
tsReset=0; // RESET_TS=0;
break;
}
case VR_CONFIG: // write bytes to SPI interface
case VR_EEPROM_BIASGEN_BYTES: // falls through and actual command is tested below
{
// the value bytes are the specific config command
// the index bytes are the arguments
// more data comes in the setupdat
SYNCDELAY;
value = SETUPDAT[2]; // Get request value
value |= SETUPDAT[3] << 8; // data comes little endian
ind = SETUPDAT[4]; // Get index
ind |= SETUPDAT[5] << 8;
len = SETUPDAT[6]; // length for data phase
len |= SETUPDAT[7] << 8;
switch(value&0xFF){ // take LSB for specific setup command because equalizer uses MSB for channel
// final short CMD_IPOT = 1, CMD_RESET_EQUALIZER = 2, CMD_SCANNER = 3, CMD_EQUALIZER = 4, CMD_SETBIT = 5, CMD_VDAC = 6;
#define CMD_IPOT 1
#define CMD_RESET_EQUALIZER 2
#define CMD_SCANNER 3
#define CMD_EQUALIZER 4
#define CMD_SETBIT 5
#define CMD_VDAC 6
#define CMD_INITDAC 7
case CMD_IPOT:
selectIPots;
numBiasBytes=len;
while(len){ // Move new data through EP0OUT, one packet at a time,
// eventually will get len down to zero by bc=64,64,15 (for example)
// Arm endpoint - do it here to clear (after sud avail)
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
SYNCDELAY;
while(EP0CS & bmEPBUSY); // spin here until data arrives
bc = EP0BCL; // Get the new bytecount
for(i=0; i<bc; i++){
sendConfigByte(EP0BUF[i]);
}
// value += bc; // inc eeprom value to write to, in case that's what we're doing
len -= bc; // dec total byte count
}
toggleLatch();
selectNone;
LED=!LED;
break;
case CMD_VDAC:
// EP0BUF has b0=channel (same for each DAC), b1=DAC1 MSB, b2=DAC1 LSB, b3=DAC0 MSB, b4=DAC0 LSB
if(len!=6) return TRUE; // error, should have 6 bytes which are just written out to DACs surrounded by dacNSync=0
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
SYNCDELAY;
while(EP0CS & bmEPBUSY); // spin here until data arrives
startDACSync();
for(i=0;i<6;i++){
sendDACByte(EP0BUF[i]);
}
endDACSync();
//toggleLDAC();
LED=!LED;
break;
case CMD_INITDAC:
initDAC();
LED=!LED;
break;
case CMD_SETBIT:
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
SYNCDELAY;
while(EP0CS & bmEPBUSY); // spin here until data arrives
// sends value=CMD_SETBIT, index=portbit with (port(b=0,d=1,e=2)<<8)|bitmask(e.g. 00001000) in MSB/LSB, byte[0]=value (1,0)
// also if button is tristable type in GUI, then byte[0] has tristate in bit1
{
bit bitval=(EP0BUF[0]&1); // 1=set, 0=clear
bit tristate=(EP0BUF[0]&2?1:0); // 1=tristate, 0=drive
unsigned char bitmask=SETUPDAT[4]; // bitmaskit mask, LSB of ind
switch(SETUPDAT[5]){ // this is port, MSB of ind
case 0: // port c
if(bitval) IOC|=bitmask; else IOC&= ~bitmask;
if(tristate) OEC&= ~bitmask; else OEC|=bitmask;
break;
case 1: // port d
if(bitval) IOD|=bitmask; else IOD&= ~bitmask;
if(tristate) OED&= ~bitmask; else OED|=bitmask;
break;
case 2: // port e
if(bitval) IOE|=bitmask; else IOE&= ~bitmask;
if(tristate) OEE&= ~bitmask; else OEE|=bitmask;
break;
default:
return TRUE; // error
}
LED=!LED;
}
break;
case CMD_SCANNER:
// index=1, continuous, index=0 go to channel
// Arm endpoint - do it here to clear (after sud avail) and get the data for channel to scan to if there is one. in any case must read data
// or subsequent requests will fail.
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
SYNCDELAY;
while(EP0CS & bmEPBUSY); // spin here until data arrives
if(ind==0){ // go to channel
ET2=0; // disable timer2 interrupt - IE.5
TR2=0; // stop timer2
i=255; // timeout on scanner clear
while(IOE&ScanSync && i-->0){ // clock scanner to end and timeout if there is no chip there
scanClock=1; // sync happens on falling edge
_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
scanClock=0;
_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
}
if(i==0) return TRUE; // scan to start failed
bc = EP0BUF[0]; // Get the channel number to scan to
for(i=0; i<bc; i++){
scanClock=1; _nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
scanClock=0; _nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
}
}else{ // continuous scanning
RCAP2L=0xff-EP0BUF[0]; // load timer 2 low byte reload register with 0xff-period. period=0 reload is 0xff00 (255 counts), period=255, reload is 0x0000, period=64k
ET2=1; // enable timer2 interrupt - this is IE.5 bit addressable
TR2=1; // run timer2
}
LED=!LED;
break;
case CMD_EQUALIZER:
/*
the scheme right now for loading the AERKillBit and the local Vq's go as follows,
start with AddSel, which has 7 bits, RX0 to RX6, toggle bitlatch low/high - this signal
latches the bits for the decoder.
The output of the decoder is not activated till DataSel is chosen, the 10 bits are loaded, 5 bits
for Vq of SOS and 5bits for Iq of bpf, then when bitlatch is toggled low/high, then the
output of the decoder is released.
During this toggle of latch, the selected channel will also latch in the value on AERKillBit.
The only thing that I'm worrying about right now is that this value has to be remembered somewhere,
i.e. if I choose channels 10, 15 neurons to be inactivated, then even if I choose
new values for Vq and Iq, this information has to be stored somewhere. The
AERKillBit in essence is like an additional bit to the bits for the DataSel.
*/
// value has cmd in LSB, channel in MSB
// index has b11=bpfkilled, b10=lpfkilled, b9-5=qbpf, b4-0=qsos
/*All other 16-bit and 32-bit values are stored, contrary to other Intel
processors, in big endian format, with the high-order byte stored first. For
example, the LJMP and LCALL instructions expect 16-bit addresses that are
in big endian format.
*/
// index is channel address, bytes={gain,quality,killed (1=killed,0=active)}
selectAddr;
sendConfigBits(SETUPDAT[3],7); // send 7 bit address
toggleLatch();
_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
selectNone;
_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();
selectData;
sendConfigBits(SETUPDAT[4]&0x1f,5); // what is this for?
sendConfigBits((SETUPDAT[4]>>5)|(SETUPDAT[5]<<3),5);
/* commented out because of bug in cochleaams1b where select of a single kill bit is inverted so everybody but the one you want
is selected. however, the equalizer DAC current splitters still work
// set each killbit
selectLPFKill; // clears ybit
if(SETUPDAT[5]&4){ // kill LPF
aerKillBit=0; // hack
}else{
aerKillBit=0;
}
toggleLatch();
selectBPFKill; // sets ybit
if(SETUPDAT[5]&8){ // kill BPF
aerKillBit=0; // hack
}else{
aerKillBit=0;
}
*/
toggleLatch();
selectNone;
LED=!LED;
break;
case CMD_RESET_EQUALIZER:
return TRUE; // not yet implmented
LED=!LED;
break;
default:
return(TRUE); // don't recognize command
}
EP0BCH = 0;
EP0BCL = 0; // Arm endpoint with 0 byte to transfer
return(FALSE); // very important, otherwise get stall
}
case VR_SET_POWERDOWN: // control powerDown output bit
{
if (SETUPDAT[2])
{
powerDown=1;
} else
{
powerDown=0;
}
*EP0BUF=VR_SET_POWERDOWN;
SYNCDELAY;
EP0BCH = 0;
EP0BCL = 1; // Arm endpoint with 1 byte to transfer
SYNCDELAY;
EP0CS |= bmHSNAK; // Acknowledge handshake phase of device request
break; // very important, otherwise get stall
}
/*
case VR_SETARRAYRESET: // set array reset, based on lsb of argument
{
if (SETUPDAT[2]&0x01)
{
IOE=IOE|ARRAY_RESET_MASK; //IOE|=arrayReset;
} else
{
IOE=IOE&NOT_ARRAY_RESET_MASK;
}
*EP0BUF=VR_SETARRAYRESET;
SYNCDELAY;
EP0BCH = 0;
EP0BCL = 1; // Arm endpoint with 1 byte to transfer
EP0CS |= bmHSNAK; // Acknowledge handshake phase of device request
return(FALSE); // very important, otherwise get stall
}
case VR_DOARRAYRESET: // reset array for fixed reset time
{
IOE=IOE&NOT_ARRAY_RESET_MASK;
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_(); // a few us
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
IOE=IOE|ARRAY_RESET_MASK; //IOE|=arrayReset;
*EP0BUF=VR_DOARRAYRESET;
SYNCDELAY;
EP0BCH = 0;
EP0BCL = 1; // Arm endpoint with 1 byte to transfer
EP0CS |= bmHSNAK; // Acknowledge handshake phase of device request
return (FALSE); // very important, otherwise get stall
}
*/
/* case VR_TIMESTAMP_TICK:
{
if (SETUPDAT[0]==VR_UPLOAD) //1010_0000 :vendor request to device, direction IN
{
EP0BUF[0] = SETUPDAT[1];
EP0BUF[1]= operationMode;
EP0BCH = 0;
EP0BCL = 2;
EP0CS |= bmHSNAK;
} else
{
operationMode=SETUPDAT[2];
if (operationMode==0)
{
TIMESTAMP_MODE = 0;
CFG_TIMESTAMP_COUNTER = 0;
}else if (operationMode==1)
{
CFG_TIMESTAMP_COUNTER = 1;
TIMESTAMP_MODE = 0;
}else if (operationMode==2)
{
CFG_TIMESTAMP_COUNTER = 0;
TIMESTAMP_MODE = 1;
}else if (operationMode==3)
{
CFG_TIMESTAMP_COUNTER = 1;
TIMESTAMP_MODE = 1;
}
*EP0BUF = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
}
return(FALSE);
}*/
case VR_IS_TS_MASTER:
{
EP0BUF[0] = SETUPDAT[1];
EP0BUF[1]= TIMESTAMP_MASTER;
EP0BCH = 0;
EP0BCL = 2;
EP0CS |= bmHSNAK;
return(FALSE);
}
/* case VR_MISSED_EVENTS:
{
EX1=0;
EP0BUF[0] = SETUPDAT[1];
EP0BUF[4]= (missedEvents & 0xFF000000) >> 24;
EP0BUF[3]= (missedEvents & 0x00FF0000) >> 16;
EP0BUF[2]= (missedEvents & 0x0000FF00) >> 8;
EP0BUF[1]= missedEvents & 0x000000FF;
EP0BCH = 0;
EP0BCL = 5;
EP0CS |= bmHSNAK;
missedEvents=0;
EX1=1;
return(FALSE);
}*/
case VR_RAM:
case VR_EEPROM:
{
value = SETUPDAT[2]; // Get address and length
value |= SETUPDAT[3] << 8;
len = SETUPDAT[6];
len |= SETUPDAT[7] << 8;
// Is this an upload command ?
if(SETUPDAT[0] == VR_UPLOAD) // this is automatically defined on host from direction of vendor request
{
while(len) // Move requested data through EP0IN
{ // one packet at a time.
while(EP0CS & bmEPBUSY);
if(len < EP0BUFF_SIZE)
bc = len;
else
bc = EP0BUFF_SIZE;
// Is this a RAM upload ?
if(SETUPDAT[1] == VR_RAM)
{
for(i=0; i<bc; i++)
*(EP0BUF+i) = *((BYTE xdata *)value+i);
}
else
{
for(i=0; i<bc; i++)
*(EP0BUF+i) = 0xcd;
EEPROMRead(value,(WORD)bc,(WORD)EP0BUF);
}
EP0BCH = 0;
EP0BCL = (BYTE)bc; // Arm endpoint with # bytes to transfer
value += bc;
len -= bc;
}
}
// Is this a download command ?
else if(SETUPDAT[0] == VR_DOWNLOAD) // this is automatically defined on host from direction of vendor request
{
while(len) // Move new data through EP0OUT
{ // one packet at a time.
// Arm endpoint - do it here to clear (after sud avail)
EP0BCH = 0;
EP0BCL = 0; // Clear bytecount to allow new data in; also stops NAKing
while(EP0CS & bmEPBUSY);
bc = EP0BCL; // Get the new bytecount
// Is this a RAM download ?
if(SETUPDAT[1] == VR_RAM)
{
for(i=0; i<bc; i++)
*((BYTE xdata *)value+i) = *(EP0BUF+i);
}
else
EEPROMWrite(value,bc,(WORD)EP0BUF);
value += bc;
len -= bc;
}
}
return(FALSE);
}
default:
{ // we received an invalid command
return(TRUE);
}
}
*EP0BUF = SETUPDAT[1];
EP0BCH = 0;
EP0BCL = 1;
EP0CS |= bmHSNAK;
return(FALSE);
}
void EEPROM_Read()
{
uint32_t len, check, sum;
uint8_t i;
uint32_t data[Data_Len];
PID_Pitch.P = 2.0;
PID_Pitch.I = 0.0;
PID_Pitch.D = 40.0;
PID_Pitch.windupGuard = 50;
PID_Roll.P = 2.0;
PID_Roll.I = 0.0;
PID_Roll.D = 40.0;
PID_Roll.windupGuard = 50;
PID_Yaw.P = 7.0;
PID_Yaw.I = 0.0;
PID_Yaw.D = 5.0;
PID_Yaw.windupGuard = 50;
PID_Altitude.P = 0.5;
PID_Altitude.I = 0.007;
PID_Altitude.D = 0.008;
PID_Altitude.windupGuard = 500;
PID_PosX.P = 0.2;
PID_PosX.I = 0.0;
PID_PosX.D = 0.1;
PID_PosX.windupGuard = 5.0;
PID_PosY.P = 0.2;
PID_PosY.I = 0.0;
PID_PosY.D = 0.1;
PID_PosY.windupGuard = 5.0;
PID_Attenuation.P = 0.0;
PID_Attenuation.I = 0.0;
PID_Attenuation.D = 0.0;
Att_Offset.Roll = 2.492;
Att_Offset.Pitch = -1.885;
Gyro_Offset.X = 0;
Gyro_Offset.Y = 0;
Gyro_Offset.Z = 0;
Accel_Offset.X = 0;
Accel_Offset.Y = 0;
Accel_Offset.Z = 0;
Xs = 1.0;
Ys = 1.0;
Xb = 0.0;
Yb = 0.0;
EEPROMRead(&len, EEPROM_STARTADDRESS, 4);
if (len != Data_Len)
{
UARTprintf("EEPROM length error.\nPID parameter reset to defult.\n");
EEPROM_Write();
return ;
}
EEPROMRead(data, EEPROM_STARTADDRESS + 4, 4*len );
EEPROMRead(&sum, EEPROM_STARTADDRESS + 4 + 4*len, 4);
check = len;
for( i = 0 ; i < len ; i++)
check += *(data+i);
if (check == sum)
{
UARTprintf("EEPROM data loaded.\n");
PID_Roll.P = (float)data[0] / 100;
PID_Roll.I = (float)data[1] / 1000;
PID_Roll.D = (float)data[2] / 100;
PID_Pitch.P = (float)data[3] / 100;
PID_Pitch.I = (float)data[4] / 1000;
PID_Pitch.D = (float)data[5] / 100;
PID_Yaw.P = (float)data[6] / 100;
PID_Yaw.I = (float)data[7] / 1000;
PID_Yaw.D = (float)data[8] / 100;
PID_Altitude.P = (float)data[9] / 100;
PID_Altitude.I = (float)data[10] / 10000;
PID_Altitude.D = (float)data[11] / 1000;
PID_PosX.P = (float)data[12] / 100;
PID_PosX.I = (float)data[13] / 10000;
PID_PosX.D = (float)data[14] / 1000;
PID_PosY.P = (float)data[12] / 100;
PID_PosY.I = (float)data[13] / 10000;
PID_PosY.D = (float)data[14] / 1000;
PID_Attenuation.P = (float)data[15] / 100;
PID_Attenuation.I = (float)data[16] / 1000;
PID_Attenuation.D = (float)data[17] / 100;
Att_Offset.Roll = (float)((int32_t)data[18]) / 1000;
Att_Offset.Pitch = (float)((int32_t)data[19]) / 1000;
Gyro_Offset.X = ((int32_t)data[20]);
Gyro_Offset.Y = ((int32_t)data[21]);
Gyro_Offset.Z = ((int32_t)data[22]);
Accel_Offset.X = ((int32_t)data[23]);
Accel_Offset.Y = ((int32_t)data[24]);
Accel_Offset.Z = ((int32_t)data[25]);
Xs = (float)data[26] / 1000;
Ys = (float)data[27] / 1000;
Xb = (float)data[28] / 1000;
Yb = (float)data[29] / 1000;
}
else
{
UARTprintf("EEPROM data check error.\nPID parameter reset to defult.\n");
EEPROM_Write();
}
}
