/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Initializes the GPT drivers 2 and 3.
*/
gptStart(&GPTD2, &gpt2cfg);
gptPolledDelay(&GPTD2, 10); /* Small delay.*/
gptStart(&GPTD3, &gpt3cfg);
gptPolledDelay(&GPTD3, 10); /* Small delay.*/
/*
* Normal main() thread activity, it changes the GPT1 period every
* five seconds.
*/
while (TRUE) {
palSetPad(GPIOD, GPIOD_LED4);
gptStartContinuous(&GPTD2, 5000);
chThdSleepMilliseconds(5000);
gptStopTimer(&GPTD2);
palClearPad(GPIOD, GPIOD_LED4);
gptStartContinuous(&GPTD2, 2500);
chThdSleepMilliseconds(5000);
gptStopTimer(&GPTD2);
}
}
//-----------------------------------------------------------------------------
// polled read of a register
uint8_t
kbv_readRegister(VectorNavDriver * nvp, uint8_t reg, uint8_t size, uint8_t * buf)
{
uint8_t r[] = { 0x01, reg, 0x00, 0x00 };
chSysLock();
spiSelectI(nvp->spip);
// we ignore the first exchange's return
for ( int i = 0 ; i < 4 ; ++i )
spiPolledExchange(nvp->spip, r[i]);
spiUnselectI(nvp->spip);
gptPolledDelay(nvp->gpdp, 50);
spiSelectI(nvp->spip);
for ( int i = 0 ; i < 4 ; ++i )
r[i] = spiPolledExchange(nvp->spip, 0);
// was there an error?
if ( r[0] == 0x00 && r[1] == 0x01 && r[2] == reg && r[3] == 0x00 )
{
// all good!
for ( int i = 0 ; i < size ; ++i )
buf[i] = spiPolledExchange(nvp->spip, 0);
}
spiUnselectI(nvp->spip);
gptPolledDelay(nvp->gpdp, 50);
chSysUnlock();
return r[3];
}
//Release the SCCB -- requires the timer
static void stopCondition(void){
//chSysLockFromIsr();
//Pull SDA Low
palWritePad(CAM_CTL_PORT, CAM_SDA, 0);
//Set SCL Low
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
//SCL High
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
//SDA High
palWritePad(CAM_CTL_PORT, CAM_SDA, 1);
//chSysUnlockFromIsr();
return;
}
//Initiate SCCB start condition -- requires the timer
static void startCondition(void){
//chSysLockFromIsr();
//Pull SDA SCL High
palWritePad(CAM_CTL_PORT, CAM_SDA, 1);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Wait
gptPolledDelay(&GPTD3,CLK_DELAY);
//Data Low
palWritePad(CAM_CTL_PORT, CAM_SDA, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
//Clock Low
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
//chSysUnlockFromIsr();
return;
}
//Configure the camera
msg_t configureCam(void){
uint8_t tmp;
idleState();
//Poweron the camera
wakeupCam();
gptPolledDelay(&GPTD3,PWR_DELAY);
//Set Default Values
cameraWriteCycle( CAM_COM7, 0x80 );
cameraWriteCycle( CAM_CLKRC, 0x80 );
cameraWriteCycle( CAM_COM8, 0x00 ); //Turn off AGC, AWB, AEC for config
//Configure the camera
for( tmp = 0; tmp < CONFIG_PAIRS; tmp++ )
cameraWriteCycle( cam_config[tmp][0], cam_config[tmp][1] );
tmp = checkCameraSanity();
//powerdownCam();
return (msg_t)tmp;
}
static uint8_t readByte(void){
//chSysLockFromIsr();
//Configure input pad
uint8_t byte = 0;
palSetPadMode(CAM_CTL_PORT, CAM_SDA, PAL_MODE_INPUT );
gptPolledDelay(&GPTD3,CLK_DELAY);
//MSB Bit 7
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<7;
//Bit 6
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<6;
//Bit 5
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<5;
//Bit 4
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<4;
//Bit 3
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<3;
//Bit 2
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<2;
//Bit 1
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) )<<1;
//Bit 0
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
byte |= ( palReadPad(CAM_CTL_PORT, CAM_SDA) );
//9th Don't Care Byte
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
//Dive to 1 per datasheet
palSetPadMode(CAM_CTL_PORT, CAM_SDA, PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_LOWEST );
palWritePad(CAM_CTL_PORT, CAM_SDA, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
//chSysUnlockFromIsr();
//Re-assert the SDA
palSetPadMode(CAM_CTL_PORT, CAM_SDA, PAL_MODE_OUTPUT_OPENDRAIN |
PAL_STM32_OSPEED_LOWEST );
//Done with byte
return byte;
}
//Write a byte, MSB first, with don't-care at the end -- requires the timer
static void writeByte(uint8_t byte){
//chSysLockFromIsr();
//MSB Bit 7
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x80)>>7);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 6
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x40)>>6);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 5
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x20)>>5);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 4
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x10)>>4);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 3
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x08)>>3);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 2
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x04)>>2);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 1
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x02)>>1);
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//Bit 0
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
palWritePad(CAM_CTL_PORT, CAM_SDA, (byte & 0x01) );
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
//9th Don't Care bit
gptPolledDelay(&GPTD3,CLK_DELAY);
palWritePad(CAM_CTL_PORT, CAM_SCL, 0);
gptPolledDelay(&GPTD3,CLK_DELAY);
//Deassert don't care
palSetPadMode(CAM_CTL_PORT, CAM_SDA, PAL_MODE_INPUT );
palWritePad(CAM_CTL_PORT, CAM_SCL, 1);
gptPolledDelay(&GPTD3,CLK_DELAY);
//chSysUnlockFromIsr();
//Re-assert the SDA
palSetPadMode(CAM_CTL_PORT, CAM_SDA, PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_LOWEST );
//Done with byte
return;
}