void setup() {
Wire.begin();
Serial.begin(57600);
I2Cdev::writeByte(0x68, 0x6B, 0x01); //PWR_MGMT1 for clock source as X-gyro
uint8_t temp[8] = {8, 0};//GYRO range:250, ACCEL range:2g | Refer the Datasheet if you want to change these.
I2Cdev::writeBytes(0x68, 0x1B, 2, temp); //GYRO_CFG and ACCEL_CFG
accelgyro.setRate(4);
accelgyro.setDLPFMode(0x03);
accelgyro.setFIFOEnabled(true);
I2Cdev::writeByte(0x68, 0x23, 0x78); //FIFO_EN for ACCEL,GYRO
accelgyro.setDMPEnabled(false);
I2Cdev::writeByte(0x68, 0x38, 0x11); //INT_EN
attachInterrupt(0, mpu_interrupt, RISING);
attachInterrupt(1, compass_interrupt, FALLING);
//Put the HMC5883 IC into the correct operating mode
Wire.beginTransmission(0x1E); //open communication with HMC5883
Wire.write(0x00); //select Config_Register_A:
Wire.write(0x58); //4-point avg. and 75Hz rate
Wire.write(0x02); //In Config_Register_B: +-1.9G (820LSB/G)
Wire.write(0x00); //In Mode_Register: continuous measurement mode
Wire.endTransmission();
pinMode(13, INPUT);
#ifdef CAL_DEBUG
Serial.print("Calibrating Gyros and Accel! Hold Still and Level!");
#endif
calibrate_gyros();
calibrate_accel();
accelgyro.resetFIFO();
}
// init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Copter::init_arm_motors(bool arming_from_gcs)
{
// arming marker
// Flag used to track if we have armed the motors the first time.
// This is used to decide if we should run the ground_start routine
// which calibrates the IMU
static bool did_gyro_cal = false;
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// run pre-arm-checks and display failures
if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
// reset battery failsafe
set_failsafe_battery(false);
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call update_notify a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
update_notify();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("ARMING MOTORS"));
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (ap.home_state == HOME_UNSET) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
// gyro calibration on first arming
if ((did_gyro_cal == false) && (ins.gyro_calibration_timing() == AP_InertialSensor::GYRO_CAL_STARTUP_AND_FIRST_BOOT)) {
if (!calibrate_gyros()) {
// final check that gyros calibrated successfully
if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS))) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyro calibration failed"));
AP_Notify::flags.armed = false;
failsafe_enable();
in_arm_motors = false;
return false;
}
}
did_gyro_cal = true;
}
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
set_using_interlock(true);
#else
// check if we are using motor interlock control on an aux switch
set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
#endif
// if we are using motor interlock switch and it's enabled, fail to arm
if (ap.using_interlock && motors.get_interlock()) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Interlock Enabled"));
AP_Notify::flags.armed = false;
in_arm_motors = false;
return false;
}
// if we are not using Emergency Stop switch option, force Estop false to ensure motors
// can run normally
if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)) {
set_motor_emergency_stop(false);
// if we are using motor Estop switch, it must not be in Estop position
} else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Emergency Stopped"));
AP_Notify::flags.armed = false;
in_arm_motors = false;
return false;
}
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
#if SPRAYER == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
#endif
// short delay to allow reading of rc inputs
delay(30);
// enable output to motors
enable_motor_output();
// finally actually arm the motors
motors.armed(true);
// log arming to dataflash
Log_Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
DataFlash.Log_Write_Mode(control_mode);
// reenable failsafe
failsafe_enable();
// perf monitor ignores delay due to arming
perf_ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// return success
return true;
}
