void udb_init(void)
{
defaultCorcon = CORCON ;
#if (BOARD_TYPE == UDB4_BOARD)
//<GUIOTT>
/*
* Clock speed up to 80MHz following the Mark Whitehorn branch on UDB4
* The following values have been parametrized:
* DCM frame rate: changed from hardwired 40Hz to parameter HEARTBEAT_HZ
* PID loop frame rate: changed from hardwired 40Hz to parameter PID_HZ
* ESC frame rate ESC_HZ
** note that the above three rates are not orthogonal
* Mark Whitehorn notes
* CPU clock: (limitation: UDB4 only: BOARD_TYPE not in {GREEN,RED,UDB3, RUSTYS}
* and BOARD_IS_CLASSIC_UDB==0), changed (or added) following parameters and macros
* CLK_PHASES set to 2
** FREQOSC set to 80 MHz (FCY = 40MHz)
** PWMOUTSCALE ratio of Timer 3 frequency T3FREQ to original hardwired 2 MHz
*** T3 prescaler was already at max. value of 8
** ADC_CLK speed also increases at 40MIPS since ADCLK_DIV_N_MINUS_1 was at max. value of 63
** I2CBRGVAL in magneto_udb4.c now calculated using FREQOSC and CLK_PHASES to achieve 100KHz rate
** CPU_RES defined such that cpu_timer units are .01%
** CPU_LOAD_PERCENT is changed, I think, but untested since raw cpu_timer values are already in units of .01%
** PWINSCALE is added to make pwIn values independent of CPU clock freq.
** Timer 1 period now calculated from HEARTBEAT_HZ (sets DCM rate)
*/
PLLFBDbits.PLLDIV = 38 ; // FOSC = 80 MHz (XT = 8.00MHz, N1=2, N2=2, M = 40)
CLKDIVbits.PLLPOST = 0; // N1=2
CLKDIVbits.PLLPRE = 0; // N2=2
// Clock switching to incorporate PLL
__builtin_write_OSCCONH(0x03); // Initiate Clock Switch to Primary
// Oscillator with PLL (NOSC=0b011)
__builtin_write_OSCCONL(0x01); // Start clock switching
#ifndef SIM
while (OSCCONbits.COSC != 0b011);// Wait for Clock switch to occur
while(OSCCONbits.LOCK!=1) {}; // Wait for PLL to lock
#endif
//</GUIOT>
#endif
udb_flags.B = 0 ;
#if (ANALOG_CURRENT_INPUT_CHANNEL != CHANNEL_UNUSED)
battery_current.WW = 0 ;
battery_mAh_used.WW = 0 ;
#endif
#if (ANALOG_VOLTAGE_INPUT_CHANNEL != CHANNEL_UNUSED)
battery_voltage.WW = 0 ;
#endif
#if (ANALOG_RSSI_INPUT_CHANNEL != CHANNEL_UNUSED)
rc_signal_strength = 0 ;
#endif
udb_init_leds() ;
udb_init_ADC() ;
udb_init_clock() ;
udb_init_capture() ;
#if (MAG_YAW_DRIFT == 1 || USE_BAROMETER == 1)
#if (USE_I2C1_DRIVER == 1)
I2C1_init();
#endif
#if (USE_I2C2_DRIVER == 1)
I2C2_init(); // NEW I2C QUEUE FUNCTION FOR MULTIPLE SENSOR SUPPORT
#endif
// I2C1_init() ;
#endif
udb_init_GPS() ;
udb_init_USART() ;
udb_init_pwm() ;
#if (USE_OSD == 1)
udb_init_osd() ;
#endif
SRbits.IPL = 0 ; // turn on all interrupt priorities
return ;
}
void I2C1_slave(u16 DeviceID)
{
I2C1_init(I2C1, I2C_SLAVE_MODE, DeviceID);
}
/* ----------------------------------------------------------------------------*
*
* Function Name : apds9960init
*
* Description : Configures I2C communications and initializes registers to defaults
*
* Input : None
*
* Output : None
*
* Return : True if initialized successfully. False otherwise
* ----------------------------------------------------------------------------*
* Authors: Sarath S
* Date: May 17, 2017
* ---------------------------------------------------------------------------*/
int apds9960init(void)
{
uint8_t id;
//ledSetLeftLed(LED_ON);
/* Initialize I2C */
I2C1_init();
delayms(700);
/* Read ID register and check against known values for APDS-9960 */
if( !i2c1_read(APDS9960_ID, &id,1) ) {
ledSetRightLed(LED_ON);
return false;
}
ledSetLeftLed(LED_ON);
if( !(id == APDS9960_ID_1 || id == APDS9960_ID_2) ) {
return false;
}
/* Set ENABLE register to 0 (disable all features) */
if( !setMode(ALL, OFF) ) {
return false;
}
/* Set default values for ambient light and proximity registers */
if( !i2c1_write(APDS9960_ATIME, DEFAULT_ATIME) ) {
return false;
}
if( !i2c1_write(APDS9960_WTIME, DEFAULT_WTIME) ) {
return false;
}
if( !i2c1_write(APDS9960_PPULSE, DEFAULT_PROX_PPULSE) ) {
return false;
}
if( !i2c1_write(APDS9960_POFFSET_UR, DEFAULT_POFFSET_UR) ) {
return false;
}
if( !i2c1_write(APDS9960_POFFSET_DL, DEFAULT_POFFSET_DL) ) {
return false;
}
if( !i2c1_write(APDS9960_CONFIG1, DEFAULT_CONFIG1) ) {
return false;
}
if( !setLEDDrive(DEFAULT_LDRIVE) ) {
return false;
}
if( !setProximityGain(DEFAULT_PGAIN) ) {
return false;
}
if( !setAmbientLightGain(DEFAULT_AGAIN) ) {
return false;
}
if( !setProxIntLowThresh(DEFAULT_PILT) ) {
return false;
}
if( !setProxIntHighThresh(DEFAULT_PIHT) ) {
return false;
}
if( !setLightIntLowThreshold(DEFAULT_AILT) ) {
return false;
}
if( !setLightIntHighThreshold(DEFAULT_AIHT) ) {
return false;
}
if( !i2c1_write(APDS9960_PERS, DEFAULT_PERS) ) {
return false;
}
if( !i2c1_write(APDS9960_CONFIG2, DEFAULT_CONFIG2) ) {
return false;
}
if( !i2c1_write(APDS9960_CONFIG3, DEFAULT_CONFIG3) ) {
return false;
}
/* Set default values for gesture sense registers */
if( !setGestureEnterThresh(DEFAULT_GPENTH) ) {
return false;
}
if( !setGestureExitThresh(DEFAULT_GEXTH) ) {
return false;
}
if( !i2c1_write(APDS9960_GCONF1, DEFAULT_GCONF1) ) {
return false;
}
if( !setGestureGain(DEFAULT_GGAIN) ) {
return false;
}
if( !setGestureLEDDrive(DEFAULT_GLDRIVE) ) {
return false;
}
if( !setGestureWaitTime(DEFAULT_GWTIME) ) {
return false;
}
if( !i2c1_write(APDS9960_GOFFSET_U, DEFAULT_GOFFSET) ) {
return false;
}
if( !i2c1_write(APDS9960_GOFFSET_D, DEFAULT_GOFFSET) ) {
return false;
}
if( !i2c1_write(APDS9960_GOFFSET_L, DEFAULT_GOFFSET) ) {
return false;
}
if( !i2c1_write(APDS9960_GOFFSET_R, DEFAULT_GOFFSET) ) {
return false;
}
if( !i2c1_write(APDS9960_GPULSE, DEFAULT_GPULSE) ) {
return false;
}
if( !i2c1_write(APDS9960_GCONF3, DEFAULT_GCONF3) ) {
return false;
}
if( !setGestureIntEnable(DEFAULT_GIEN) ) {
return false;
}
#if 0
/* Gesture config register dump */
uint8_t reg;
uint8_t val;
for(reg = 0x80; reg <= 0xAF; reg++) {
if( (reg != 0x82) && \
(reg != 0x8A) && \
(reg != 0x91) && \
(reg != 0xA8) && \
(reg != 0xAC) && \
(reg != 0xAD) )
{
i2c1_read(reg, val,1);
//debugPutChar(reg);
debugPutString(": 0x");
//debugPutChar(val);
}
}
for(reg = 0xE4; reg <= 0xE7; reg++) {
i2c1_read(reg, val,1);
//debugPutChar(reg);
debugPutString(": 0x");
//debugPutChar(val);
}
#endif
return true;
}/* End of this function */
