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 }
/** * 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 }
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); }