Esempio n. 1
0
void setup(void)
{
    board.init();

    // sleep for 100ms
    board.delayMilliseconds(100);

    // flash the LEDs to indicate startup
    board.ledRedOn();
    board.ledGreenOff();
    for (uint8_t i = 0; i < 10; i++) {
        board.ledRedToggle();
        board.ledGreenToggle();
        board.delayMilliseconds(50);
    }
    board.ledRedOff();
    board.ledGreenOff();

    // initialize our RC, IMU, mixer, and PID controller
    rc.init(&board);
    imu.init(&board);
    pid.init();
    mixer.init(&board, &rc, &pid); 
    msp.init(&board, &imu, &mixer, &rc);

    // set initial time
    previousTime = board.getMicros();

    // always do gyro calibration at startup
    calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;

    // trigger accelerometer calibration requirement
    haveSmallAngle = true;

} // setup
Esempio n. 2
0
void setup(void)
{
    uint32_t calibratingGyroMsec;

    // Get particulars for board
    board.init(imuLooptimeUsec, calibratingGyroMsec);

    // sleep for 100ms
    board.delayMilliseconds(100);

    // flash the LEDs to indicate startup
    board.ledRedOn();
    board.ledGreenOff();
    for (uint8_t i = 0; i < 10; i++) {
        board.ledRedToggle();
        board.ledGreenToggle();
        board.delayMilliseconds(50);
    }
    board.ledRedOff();
    board.ledGreenOff();

    // compute cycles for calibration based on board's time constant
    calibratingGyroCycles = (uint16_t)(1000. * calibratingGyroMsec / imuLooptimeUsec);
    calibratingAccCycles  = (uint16_t)(1000. * CONFIG_CALIBRATING_ACC_MSEC  / imuLooptimeUsec);

    // initializing timing tasks
    imuTask.init(imuLooptimeUsec);
    rcTask.init(CONFIG_RC_LOOPTIME_MSEC * 1000);
    accelCalibrationTask.init(CONFIG_CALIBRATE_ACCTIME_MSEC * 1000);
    altitudeEstimationTask.init(CONFIG_ALTITUDE_UPDATE_MSEC * 1000);

    // initialize our external objects with objects they need
    rc.init(&board);
    stab.init(&rc, &imu);
    imu.init(&board, calibratingGyroCycles, calibratingAccCycles);
    mixer.init(&board, &rc, &stab); 
    msp.init(&board, &imu, &nav, &mixer, &rc);
    nav.init(&board, &imu, &baro, &rc);

    // always do gyro calibration at startup
    calibratingG = calibratingGyroCycles;

    // assume shallow angle (no accelerometer calibration needed)
    haveSmallAngle = true;

    // ensure not armed
    armed = false;
    
    // attempt to initialize barometer
    baro.init(&board);

} // setup