int8_t initSensors(){

	int8_t err1 = Accelerometer_init();
	int8_t err2 = Compass_init();
	int8_t err3 = Gyroscope_init();
	err3 = getGyroCalibrationOffset();

	if (err1) {
		return err1;
	}
	if (err2) {
		return err2;
	}
	if (err3) {
		return err3;
	}
	return NO_ERR; //dont know how to return multiple errors
}
Ejemplo n.º 2
0
Archivo: Compas.c Proyecto: sdajani/sdp
/**
 * Function: initMasterSM
 * @return None.
 * @remark Initializes the master state machine for the command canter.
 * @author David Goodman
 * @date 2013.03.10  */
void initMasterSM() {
    Board_init();
    Serial_init();
    Timer_init();

    // CC buttons
    LOCK_BUTTON_TRIS = 1;
    ZERO_BUTTON_TRIS = 1;

    Encoder_init();

    #ifdef USE_XBEE
    Xbee_init();
    #endif

    #ifdef USE_NAVIGATION
    Navigation_init();
    #endif

    #ifdef USE_ENCODERS
    I2C_init(I2C_BUS_ID, I2C_CLOCK_FREQ);
    #endif


    #ifdef USE_ACCELEROMETER
    //printf("Initializing accelerometer...\n");
    if (Accelerometer_init() != SUCCESS) {
        printf("Failed to initialize the accelerometer.\n");
        //return;
    }
    //printf("Initialized the accelerometer.\n");

    // Configure ports as outputs
    LED_N_TRIS = OUTPUT;
    LED_S_TRIS = OUTPUT;
    LED_E_TRIS = OUTPUT;
    LED_W_TRIS = OUTPUT;


    Timer_new(TIMER_ACCELEROMETER, LED_DELAY );
    #endif
}
Ejemplo n.º 3
0
int main(void) {
    // Initializations
    Board_init();
    Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
    I2C_init(I2C_ID, I2C_CLOCK_FREQ);
    Magnetometer_init();
    Interface_init();
    DELAY(5);
    if (Accelerometer_init() != SUCCESS) {
        Interface_errorLightOn();
        dbprint("Failed accel. init.\n");
        return FAILURE;
    }
    dbprint("Ready to calibrate.\n");
    Interface_readyLightOn();
    DELAY(STARTUP_DELAY);

    LCD_setPosition(0,0);
    dbprint("Please level scope.\n");
    Interface_pitchLightsOn();
    state = CALIBRATE_PITCH;
    Timer_new(TIMER_TEST2, PRINT_DELAY);
    bool holdingCalibrate = FALSE;

    while (1) {
        switch (state) {
            case CALIBRATE_PITCH:
                if (Accelerometer_isLevel() && !holdingCalibrate) {
                    holdingCalibrate = TRUE;
                    Timer_new(TIMER_TEST, CALIBRATE_HOLD_DELAY);
                    Interface_readyLightOff();
                    Interface_waitLightOn();
                    LCD_setPosition(1,0);
                    dbprint("Hold level...\n");
                }
                if (Accelerometer_isLevel() && holdingCalibrate && Timer_isExpired(TIMER_TEST)) {
                    LCD_setPosition(1,0);
                    dbprint("Pitch done!\n");
                    Interface_pitchLightsOff();
                    Interface_readyLightOn();
                    Interface_waitLightOff();
                    holdingCalibrate = FALSE;
                    DELAY(STARTUP_DELAY);

                    state = CALIBRATE_YAW;
                    LCD_setPosition(0,0);
                    dbprint("Turn scope north.\n                    \n");
                    Interface_yawLightsOn();
                }
                else if (!Accelerometer_isLevel() && holdingCalibrate) {
                    holdingCalibrate = FALSE;
                    Timer_stop(TIMER_TEST);
                    LCD_setPosition(1,0);
                    dbprint("               \n");
                    Interface_readyLightOn();
                    Interface_waitLightOff();
                }
                break;
            case CALIBRATE_YAW:
                if (Magnetometer_isNorth() && !holdingCalibrate) {
                    holdingCalibrate = TRUE;
                    Timer_new(TIMER_TEST, CALIBRATE_HOLD_DELAY);
                    Interface_readyLightOff();
                    Interface_waitLightOn();
                    LCD_setPosition(1,0);
                    dbprint("Hold north + level..\n");
                }
                if (Magnetometer_isNorth() && holdingCalibrate && Timer_isExpired(TIMER_TEST)) {
                    LCD_setPosition(1,0);
                    dbprint("Yaw done!\n");
                    Interface_yawLightsOff();
                    Interface_readyLightOn();
                    Interface_waitLightOff();
                    holdingCalibrate = FALSE;
                    DELAY(STARTUP_DELAY);
                    return SUCCESS;
                }
                else if (!Magnetometer_isNorth() && holdingCalibrate) {
                    holdingCalibrate = FALSE;
                    Timer_stop(TIMER_TEST);
                    LCD_setPosition(1,0);
                    dbprint("               \n");
                    Interface_readyLightOn();
                    Interface_waitLightOff();
                }
                #ifdef DEBUG_MAGNETOMETER
                if (Timer_isExpired(TIMER_TEST2)) {
                    LCD_setPosition(3,0);
                    dbprint("Mag: %.2f\n",Magnetometer_getHeading());
                    Timer_new(TIMER_TEST2, PRINT_DELAY);
                }
                #endif
                break;
        }
        Magnetometer_runSM();
        Accelerometer_runSM();
        Interface_runSM();
    }

  

    return (SUCCESS);
}