/* ********************************************************************** */ /* ArduCopter Quadcopter code */ /* */ /* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ /* IMU DCM code from Diydrones.com */ /* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */ /* Ardupilot core code : from DIYDrones.com development team */ /* Authors : Arducopter development team */ /* Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, */ /* Jani Hirvinen, Ken McEwans, Roberto Navoni, */ /* Sandro Benigno, Chris Anderson */ /* Date : 04-07-2010 */ /* Version : 1.3 beta */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* Mounting position : RC connectors pointing backwards */ /* This code use this libraries : */ /* APM_RC : Radio library (with InstantPWM) */ /* APM_ADC : External ADC library */ /* DataFlash : DataFlash log library */ /* APM_BMP085 : BMP085 barometer library */ /* APM_Compass : HMC5843 compass library [optional] */ /* GPS_UBLOX or GPS_NMEA: GPS library [optional] */ /* ********************************************************************** */ /* **** Switch Functions ***** AUX1 ON = Stable Mode AUX1 OFF = Acro Mode GEAR ON = GPS Hold GEAR OFF = Flight Assist (Stable Mode) **** LED Feedback **** Green LED On = APM Initialization Finished Yellow LED On = GPS Hold Mode Yellow LED Off = Flight Assist Mode (No GPS) Red LED On = GPS Fix Red LED Off = No GPS Fix */ #include #include #include #include #include // Put your GPS library here: #include // MTK GPS //#include // EEPROM storage for user configurable values #include #include "UserSettings.h" /* APM Hardware definitions */ #define LED_Yellow 36 #define LED_Red 35 #define LED_Green 37 #define RELE_pin 47 #define SW1_pin 41 #define SW2_pin 40 /* *** */ /* ***************************************************************************** */ /* CONFIGURATION PART */ /* ***************************************************************************** */ // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 // Tested value : 408 #define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi // IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : #define Gyro_Gain_X 0.4 //X axis Gyro gain #define Gyro_Gain_Y 0.41 //Y axis Gyro gain #define Gyro_Gain_Z 0.41 //Z axis Gyro gain #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second /*For debugging purposes*/ #define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ int SENSOR_SIGN[]={1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1}; int AN[6]; //array that store the 6 ADC channels int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers float G_Dt=0.02; // Integration time for the gyros (DCM algorithm) float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector float Accel_Vector_unfiltered[3]= {0,0,0}; //Store the acceleration in a vector //float Accel_magnitude; //float Accel_weight; float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data float Omega_P[3]= {0,0,0};//Omega Proportional correction float Omega_I[3]= {0,0,0};//Omega Integrator float Omega[3]= {0,0,0}; float errorRollPitch[3]= {0,0,0}; float errorYaw[3]= {0,0,0}; float errorCourse=0; float COGX=0; //Course overground X axis float COGY=1; //Course overground Y axis float roll=0; float pitch=0; float yaw=0; unsigned int counter=0; float DCM_Matrix[3][3]= { {1,0,0} ,{0,1,0} ,{0,0,1} }; float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here float Temporary_Matrix[3][3]={ {0,0,0} ,{0,0,0} ,{0,0,0} }; // GPS variables float speed_3d=0; int GPS_ground_speed=0; long timer=0; //general porpuse timer long timer_old; // Attitude control variables float command_rx_roll=0; // User commands float command_rx_roll_old; float command_rx_roll_diff; float command_rx_pitch=0; float command_rx_pitch_old; float command_rx_pitch_diff; float command_rx_yaw=0; float command_rx_yaw_diff; int control_roll; // PID control results int control_pitch; int control_yaw; float K_aux; // Attitude PID controls float roll_I=0; float roll_D; float err_roll; float pitch_I=0; float pitch_D; float err_pitch; float yaw_I=0; float yaw_D; float err_yaw; //Position control long target_longitude; long target_lattitude; byte target_position; float gps_err_roll; float gps_err_roll_old; float gps_roll_D; float gps_roll_I=0; float gps_err_pitch; float gps_err_pitch_old; float gps_pitch_D; float gps_pitch_I=0; float command_gps_roll; float command_gps_pitch; //Altitude control int Initial_Throttle; int target_sonar_altitude; int err_altitude; int err_altitude_old; float command_altitude; float altitude_I; float altitude_D; // Sonar variables int Sonar_value=0; #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters int Sonar_Counter=0; // AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode) byte AP_mode = 2; long t0; int num_iter; float aux_debug; // Radio definitions int Neutro_yaw; int ch_roll; int ch_pitch; int ch_throttle; int ch_yaw; int ch_aux; int ch_aux2; #define CHANN_CENTER 1500 #define MIN_THROTTLE 1040 // Throttle pulse width at minimun... // Motor variables #define FLIGHT_MODE_+ //#define FLIGHT_MODE_X int frontMotor; int backMotor; int leftMotor; int rightMotor; byte motorArmed = 0; int minThrottle = 0; // Serial communication #define CONFIGURATOR char queryType; long tlmTimer = 0; /* ************************************************************ */ /* Altitude control... (based on sonar) */ void Altitude_control(int target_sonar_altitude) { err_altitude_old = err_altitude; err_altitude = target_sonar_altitude - Sonar_value; altitude_D = (float)(err_altitude-err_altitude_old)/G_Dt; altitude_I += (float)err_altitude*G_Dt; altitude_I = constrain(altitude_I,-100,100); command_altitude = Initial_Throttle + KP_ALTITUDE*err_altitude + KD_ALTITUDE*altitude_D + KI_ALTITUDE*altitude_I; } /* ************************************************************ */ /* Position control... */ void Position_control(long lat_dest, long lon_dest) { long Lon_diff; long Lat_diff; float gps_err_roll; float gps_err_pitch; Lon_diff = lon_dest - GPS.Longitude; Lat_diff = lat_dest - GPS.Lattitude; // ROLL gps_err_roll_old = gps_err_roll; //Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0] gps_err_roll = (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[0][0] - (float)Lat_diff*DCM_Matrix[1][0]; gps_roll_D = (gps_err_roll-gps_err_roll_old)/G_Dt; gps_roll_I += gps_err_roll*G_Dt; gps_roll_I = constrain(gps_roll_I,-500,500); command_gps_roll = KP_GPS_ROLL*gps_err_roll + KD_GPS_ROLL*gps_roll_D + KI_GPS_ROLL*gps_roll_I; command_gps_roll = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command // PITCH gps_err_pitch_old = gps_err_pitch; gps_err_pitch = -(float)Lat_diff*DCM_Matrix[0][0]- (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[1][0]; gps_pitch_D = (gps_err_pitch-gps_err_pitch_old)/G_Dt; gps_pitch_I += gps_err_pitch*G_Dt; gps_pitch_I = constrain(gps_pitch_I,-500,500); command_gps_pitch = KP_GPS_PITCH*gps_err_pitch + KD_GPS_PITCH*gps_pitch_D + KI_GPS_PITCH*gps_pitch_I; command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command } /* ************************************************************ */ // ROLL, PITCH and YAW PID controls... // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands // Stable Mode void Attitude_control() { // ROLL CONTROL if (AP_mode==2) // Normal Mode => Stabilization mode err_roll = command_rx_roll - ToDeg(roll); else err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control err_roll = constrain(err_roll,-25,25); // to limit max roll command... roll_I += err_roll*G_Dt; roll_I = constrain(roll_I,-20,20); // D term implementation => two parts: gyro part and command part // To have a better (faster) response we can use the Gyro reading directly for the Derivative term... // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected // We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command) // PID control K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I; // PITCH CONTROL if (AP_mode==2) // Normal mode => Stabilization mode err_pitch = command_rx_pitch - ToDeg(pitch); else err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command... pitch_I += err_pitch*G_Dt; pitch_I = constrain(pitch_I,-20,20); // D term pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]); // PID control K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I; // YAW CONTROL err_yaw = command_rx_yaw - ToDeg(yaw); if (err_yaw > 180) // Normalize to -180,180 err_yaw -= 360; else if(err_yaw < -180) err_yaw += 360; err_yaw = constrain(err_yaw,-60,60); // to limit max yaw command... yaw_I += err_yaw*G_Dt; yaw_I = constrain(yaw_I,-20,20); yaw_D = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]); // PID control control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; } // Acro Mode void Rate_control() { static float previousRollRate, previousPitchRate, previousYawRate; float currentRollRate, currentPitchRate, currentYawRate; // ROLL CONTROL currentRollRate = read_adc(0); // I need a positive sign here err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate; roll_I += err_roll*G_Dt; roll_I = constrain(roll_I,-20,20); roll_D = currentRollRate - previousRollRate; previousRollRate = currentRollRate; // PID control control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; // PITCH CONTROL currentPitchRate = read_adc(1); // err_pitch = ((1500-ch_pitch) * xmitFactor) - currentPitchRate; // was incorrect, inverted ELE between Arco / Stable err_pitch = ((ch_pitch - 1500) * xmitFactor) - currentPitchRate; // correct one, now ELE is ok on both modes pitch_I += err_pitch*G_Dt; pitch_I = constrain(pitch_I,-20,20); pitch_D = currentPitchRate - previousPitchRate; previousPitchRate = currentPitchRate; // PID control control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I; // YAW CONTROL currentYawRate = read_adc(2); err_yaw = ((ch_yaw-1500)* xmitFactor) - currentYawRate; yaw_I += err_yaw*G_Dt; yaw_I = constrain(yaw_I,-20,20); yaw_D = currentYawRate - previousYawRate; previousYawRate = currentYawRate; // PID control K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; } // Maximun slope filter for radio inputs... (limit max differences between readings) int channel_filter(int ch, int ch_old) { int diff_ch_old; if (ch_old==0) // ch_old not initialized return(ch); diff_ch_old = ch - ch_old; // Difference with old reading if (diff_ch_old<0) { if (diff_ch_old<-40) return(ch_old-40); // We limit the max difference between readings } else { if (diff_ch_old>40) return(ch_old+40); } //return((ch+ch_old)>>1); // Small filtering return(ch); } /* ****** SETUP ********************************************************************* */ void setup() { int i; float aux_float[3]; pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1) pinMode(LED_Red,OUTPUT); //Red LED B (PC2) pinMode(LED_Green,OUTPUT); //Green LED C (PC0) pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0) pinMode(RELE_pin,OUTPUT); // Rele output digitalWrite(RELE_pin,LOW); delay(250); APM_RC.Init(); // APM Radio initialization APM_ADC.Init(); // APM ADC library initialization DataFlash.Init(); // DataFlash log initialization GPS.Init(); // GPS Initialization readUserConfig(); // Load user configurable items from EEPROM // RC channels Initialization (Quad motors) APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped APM_RC.OutputCh(1,MIN_THROTTLE); APM_RC.OutputCh(2,MIN_THROTTLE); APM_RC.OutputCh(3,MIN_THROTTLE); if (MAGNETOMETER == 1) APM_Compass.Init(); // I2C initialization DataFlash.StartWrite(1); // Start a write session on page 1 //Serial.begin(57600); Serial.begin(115200); //Serial.println(); //Serial.println("ArduCopter Quadcopter v1.0"); // Check if we enable the DataFlash log Read Mode (switch) // If we press switch 1 at startup we read the Dataflash eeprom while (digitalRead(SW1_pin)==0) { Serial.println("Entering Log Read Mode..."); Log_Read(1,1000); delay(30000); } //delay(3000); Read_adc_raw(); delay(20); // Offset values for accels and gyros... AN_OFFSET[3] = acc_offset_x; AN_OFFSET[4] = acc_offset_y; AN_OFFSET[5] = acc_offset_z; aux_float[0] = gyro_offset_roll; aux_float[1] = gyro_offset_pitch; aux_float[2] = gyro_offset_yaw; // Take the gyro offset values for(i=0;i<300;i++) { Read_adc_raw(); for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset. { aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2; //Serial.print(AN[y]); //Serial.print(","); } //Serial.println(); Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); delay(14); } for(int y=0; y<=2; y++) AN_OFFSET[y]=aux_float[y]; Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value #ifndef CONFIGURATOR for(i=0;i<6;i++) { Serial.print("AN[]:"); Serial.println(AN_OFFSET[i]); } Serial.print("Yaw neutral value:"); Serial.println(Neutro_yaw); #endif #if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS while(1) { if (APM_RC.GetState()==1) { Serial.print("AIL:"); Serial.print(APM_RC.InputCh(0)); Serial.print("ELE:"); Serial.print(APM_RC.InputCh(1)); Serial.print("THR:"); Serial.print(APM_RC.InputCh(2)); Serial.print("YAW:"); Serial.print(APM_RC.InputCh(3)); Serial.print("AUX(mode):"); Serial.print(APM_RC.InputCh(4)); Serial.print("AUX2:"); Serial.print(APM_RC.InputCh(5)); Serial.println(); delay(200); } } #endif delay(1000); DataFlash.StartWrite(1); // Start a write session on page 1 timer = millis(); tlmTimer = millis(); Read_adc_raw(); // Initialize ADC readings... delay(20); motorArmed = 0; digitalWrite(LED_Green,HIGH); // Ready to go... } /* ***** MAIN LOOP ***** */ void loop(){ int aux; int i; float aux_float; //Log variables int log_roll; int log_pitch; int log_yaw; if((millis()-timer)>=10) // Main loop 100Hz { counter++; timer_old = timer; timer=millis(); G_Dt = (timer-timer_old)/1000.0; // Real time of loop run // IMU DCM Algorithm Read_adc_raw(); if (MAGNETOMETER == 1) { if (counter > 10) // Read compass data at 10Hz... (10 loop runs) { counter=0; APM_Compass.Read(); // Read magnetometer APM_Compass.Calculate(roll,pitch); // Calculate heading } } Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); // ***************** // Output data log_roll = ToDeg(roll)*10; log_pitch = ToDeg(pitch)*10; log_yaw = ToDeg(yaw)*10; #ifndef CONFIGURATOR Serial.print(log_roll); Serial.print(","); Serial.print(log_pitch); Serial.print(","); Serial.print(log_yaw); for (int i=0;i<6;i++) { Serial.print(AN[i]); Serial.print(","); } #endif // Write Sensor raw data to DataFlash log Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); // Write attitude to DataFlash log Log_Write_Attitude(log_roll,log_pitch,log_yaw); if (APM_RC.GetState()==1) // New radio frame? { // Commands from radio Rx... // Stick position defines the desired angle in roll, pitch and yaw ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll); ch_pitch = channel_filter(APM_RC.InputCh(1),ch_pitch); ch_throttle = channel_filter(APM_RC.InputCh(2),ch_throttle); ch_yaw = channel_filter(APM_RC.InputCh(3),ch_yaw); ch_aux = APM_RC.InputCh(4); ch_aux2 = APM_RC.InputCh(5); command_rx_roll_old = command_rx_roll; command_rx_roll = (ch_roll-CHANN_CENTER)/12.0; command_rx_roll_diff = command_rx_roll-command_rx_roll_old; command_rx_pitch_old = command_rx_pitch; command_rx_pitch = (ch_pitch-CHANN_CENTER)/12.0; command_rx_pitch_diff = command_rx_pitch-command_rx_pitch_old; aux_float = (ch_yaw-Neutro_yaw)/180.0; command_rx_yaw += aux_float; command_rx_yaw_diff = aux_float; if (command_rx_yaw > 180) // Normalize yaw to -180,180 degrees command_rx_yaw -= 360.0; else if (command_rx_yaw < -180) command_rx_yaw += 360.0; // Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains // I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now] //K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2; K_aux = K_aux*0.8 + ((ch_aux2-1500)/300.0 + 1.7)*0.2; // /300 + 1.0 if (K_aux < 0) K_aux = 0; //Serial.print(","); //Serial.print(K_aux); // We read the Quad Mode from Channel 5 if (ch_aux < 1200) { AP_mode = 1; // Position hold mode (GPS position control) digitalWrite(LED_Yellow,HIGH); // Yellow LED On } else { AP_mode = 2; // Normal mode (Stabilization assist mode) digitalWrite(LED_Yellow,LOW); // Yellow LED off } // Write Radio data to DataFlash log Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode); } // END new radio data if (AP_mode==1) // Position Control { if (target_position==0) // If this is the first time we switch to Position control, actual position is our target position { target_lattitude = GPS.Lattitude; target_longitude = GPS.Longitude; #ifndef CONFIGURATOR Serial.println(); Serial.print("* Target:"); Serial.print(target_longitude); Serial.print(","); Serial.println(target_lattitude); #endif target_position=1; //target_sonar_altitude = sonar_value; //Initial_Throttle = ch3; // Reset I terms altitude_I = 0; gps_roll_I = 0; gps_pitch_I = 0; } } else target_position=0; //Read GPS GPS.Read(); if (GPS.NewData) // New GPS data? { GPS.NewData=0; // We Reset the flag... //Output GPS data //Serial.print(","); Serial.print(GPS.Lattitude); Serial.print(","); Serial.print(GPS.Longitude); // Write GPS data to DataFlash log Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats); if (GPS.Fix) digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED else digitalWrite(LED_Red,LOW); if (AP_mode==1) { if ((target_position==1)&&(GPS.Fix)) { Position_control(target_lattitude,target_longitude); // Call position hold routine } else { //Serial.print("NOFIX"); command_gps_roll=0; command_gps_pitch=0; } } } // Control methodology selected using AUX2 if (ch_aux2 < 1200) Attitude_control(); else { Rate_control(); // Reset yaw, so if we change to stable mode we continue with the actual yaw direction command_rx_yaw = ToDeg(yaw); command_rx_yaw_diff = 0; } // Arm motor output if (ch_throttle < 1200) { control_yaw = 0; command_rx_yaw = ToDeg(yaw); command_rx_yaw_diff = 0; if (ch_yaw > 1800) { motorArmed = 1; minThrottle = 1100; } if (ch_yaw < 1200) { motorArmed = 0; minThrottle = MIN_THROTTLE; } } // Quadcopter mix // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration if (motorArmed == 1) { #ifdef FLIGHT_MODE_+ rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000); leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000); frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000); backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000); #endif #ifdef FLIGHT_MODE_X frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor #endif } if (motorArmed == 0) { rightMotor = MIN_THROTTLE; leftMotor = MIN_THROTTLE; frontMotor = MIN_THROTTLE; backMotor = MIN_THROTTLE; roll_I = 0; // reset I terms of PID controls pitch_I = 0; yaw_I = 0; // Initialize yaw command to actual yaw when throttle is down... command_rx_yaw = ToDeg(yaw); command_rx_yaw_diff = 0; } APM_RC.OutputCh(0, rightMotor); // Right motor APM_RC.OutputCh(1, leftMotor); // Left motor APM_RC.OutputCh(2, frontMotor); // Front motor APM_RC.OutputCh(3, backMotor); // Back motor // InstantPWM APM_RC.Force_Out0_Out1(); APM_RC.Force_Out2_Out3(); #ifndef CONFIGURATOR Serial.println(); // Line END #endif } #ifdef CONFIGURATOR if((millis()-tlmTimer)>=100) { readSerialCommand(); sendSerialTelemetry(); tlmTimer = millis(); } #endif }