//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++ Start Set-Up +++++++++++++++++++++++++++
// Last Update: 02-10-22
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void setup()
{
I2C.begin(); // Initialize Serial and I2C communications as MASTER
Serial.begin(9600); // 9600 data rate in bps bits/second to monitor
delay(100);
pinMode(SPKR, OUTPUT); //set the speaker as output
//Note: Port Serial1 at Teensy pins 1 (TX1) and 0 (RX1) are activated
//Note: On Teensy with Serial4.begin(9600) serial pins 17(TX4) and 16(RX4) are activated
Serial4.begin(9600);
delay(100);
if (Processor == 1) //number is set in 1
{
Serial.println("Note: Processor is No.1, the Test-Bench Teensy.");
}
if (Processor == 2)
{
Serial.println("Note: Processor is No.2, the Drone Teensy.");
}
Serial.println();
Serial.println("Check Processor Number. If correct: Press any key to continue.");
while (!Serial.available()){} // wait for a character
Serial.read(); // clear the input buffer
Serial.println();
Serial.print("Set-Up Starts.");
Serial.println();
// ............. Signal LED-Testing ............................
// Serial.print("Testing the Signal LED's.");
// Serial.println();
pwm.reset();
pwm.setPWMFreq(60); // The 60Hz set here dictates the delay's below
// Switch-OFF all 16 control LED's
for (i=0; i<16; i++)
{
pwm.setPWM(PCApin[i], 0, 4096);
}
for (i=0; i<5; i++)
{
// Switch-ON the control LEDs
pwm.setPWM(PCApin[0], 4096, 0); //Green (all sensors OK)
pwm.setPWM(PCApin[2], 4096, 0); //White (Gyro in use)
pwm.setPWM(PCApin[4], 4096, 0); //Blue (Flight Distance Ok)
pwm.setPWM(PCApin[6], 4096, 0); //Yellow (Switch on while Gyro-Drift calcul)
pwm.setPWM(PCApin[11], 4096, 0); //Red (Looptime > 60 mills)
delay(600); // For synchronisation has to be a multiple of the 60Hz freq.
// Switch-OFF the control LED
pwm.setPWM(PCApin[0], 0, 4096); //Green
pwm.setPWM(PCApin[2], 0, 4096); //White
pwm.setPWM(PCApin[4], 0, 4096); //Blue
pwm.setPWM(PCApin[6], 0, 4096); //Yellow
pwm.setPWM(PCApin[11], 0, 4096); //Red
delay(60); // For synchronisation has to be a multiple of the 60Hz freq.
}
// PCA9685 Signal LEDS PCA9685 Signsl LEDS PCA9685 Signsl LEDS
// - Green LED Pin 0 on when nDevices>=4 switched in I2C Scan
// - Green LED Pin 0 off when Flag=2 switched in Main Loop
// - Witte LED Pin 2 on when Gyro is active switched in Calc_Pitch_Roll
// - Blue LED Pin 4 Blink when GPS reads valid data switched in Main Loop
// - Yellow LED Pin 6 Not yet used
// - Red LED Pin 11 on when Loop_T_millis > 65 switched in Main Loop
// ...............................................................
Scan_4_I2C_Devices();
// ...... Barometric Airpressure using Sensor BMP-280 ..........
for (n=1; n<4; n++)
// while (start_baropressure < 900) // mbar
{
start_baropressure = Call_baro_pressure();
Serial.print("2: Barometric Start Pressure mbar = ");
Serial.println(start_baropressure,1);
delay(1000);
}
// ++++++++++++ Assign the four Servo's ++++++++++++++++++++++++
// Calib_Serie=1 : loads Bench-Unit sensor Calibration Data.
// Calib_Serie=2 : loads Drone sensor Calibration Data
if (Processor == 1) // Bench Test Teensy
{
Calib_Serie = 1;
EmCorrection =0;
}
if (Processor == 2) // Drone Teensy
{
Calib_Serie = 2;
EmCorrection = 330; // 396 is offset value measured on drone PWM to Voltage converter
}
Serial.println();
Serial.print("Note: Calibration Set No.");
Serial.print(Calib_Serie);
Serial.print(" has been loaded!!");
Serial.println();
Serial.println();
pinMode(EmVPin14, INPUT); // Prepare Analog pin 14 to read the PWM to Analog Voltages
// Link PWMpin_for_Servo_0 with Servo-Lib object Aeleron-Servo_0
Servo_0.attach(PWMpin_4_Servo_0); // Note: "4" means "for"
Servo_0.writeMicroseconds(Servo_0_In); // set servo to mid-point
// Link PWMpin_for_Servo_1 with Servo-Lib object Elevator-Servo_1
Servo_1.attach(PWMpin_4_Servo_1);
Servo_1.writeMicroseconds(Servo_1_In); // set servo to mid-point
// Link PWMpin_for_Servo_2 with Servo-Lib object Rudder-Servo_2
Servo_2.attach(PWMpin_4_Servo_2);
Servo_2.writeMicroseconds(Servo_2_In); // set servo to mid-point
// Link PWMpin_for_Servo_3 with Servo-Lib object Motor-Servo_3
Servo_3.attach(PWMpin_4_Servo_3);
Servo_3.writeMicroseconds(Servo_3_In); // set servo to mid-point
// ................................................................
// Load the Calibration Matrices of Accel.MPU6050 and MAG3110
// +++++ Configure and Calibrate MPU-6050 Accel en Gyro +++++++
MPU6050_CaliMatrix();
Config_MPU6050_Accel_and_Gyro();
if (KOMPASS == 0x0E) //MAG3110 Magnetometer
{
// +++++++ Configure and Calibrate MAG3110 Magnetometer ++++++++
// Note: MAG3110-1 (Bench) and MAG3110-2 (Drone) are the installed Magnetometer.
Mag3110_CaliMatrix();
config_MAG3110();
}
// ................................................................
// Start calculating the Gyro Drift
// Calc_GyroDrift();
// ................................................................
// Read a GPS Target for Heading Calculation
Lon_and_Lat_Targets();
Serial.println();
Serial.print("Home-Name: ");
Serial.println(HomeName);
Serial.print("Target-Name: ");
Serial.println(TargetName);
// ................................................................
Flag=0;
beep();beep();
delay(3000);
}
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
void writeTo(int DEVICE, byte address, byte val)
{
I2C.beginTransmission(DEVICE); // start transmission to device
I2C.write(address); // send register address
I2C.write(val); // send value to write
I2C.endTransmission(); // end transmission
}
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++ End Set-Up ++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++