diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index 95c9137f55..8c93f242ec 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -33,6 +33,13 @@ TODO: * ************************************************************** */ +/*************************************************************/ +// Airframe +#define QUAD 0 +#define HELI 1 + +#define AIRFRAME QUAD + /*************************************************************/ // Safety & Security @@ -137,6 +144,4 @@ TODO: #define ACCELY 4 #define ACCELZ 5 #define LASTSENSOR 6 - - - + diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index a696d47dc0..9594da1b96 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -264,6 +264,7 @@ float err_pitch; float yaw_I=0; float yaw_D; float err_yaw; +float heading_I=0; // used only by heli //Position control long target_longitude; diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index bb4ce1762d..f30c6cf694 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -182,6 +182,9 @@ //#include // ArduPilot NMEA GPS library #endif +#if AIRFRAME == HELI +#include "Heli.h" +#endif /* Software version */ #define VER 1.52 // Current software version (only numeric values) @@ -197,7 +200,8 @@ APM_Compass_Class APM_Compass; byte flightMode; -unsigned long currentTime, previousTime; +unsigned long currentTime; // current time in milliseconds +unsigned long currentTimeMicros = 0, previousTimeMicros = 0; // current and previous loop time in microseconds unsigned long mainLoop = 0; unsigned long mediumLoop = 0; unsigned long slowLoop = 0; @@ -252,13 +256,16 @@ void loop() //int i; //float aux_float; - currentTime = millis(); + currentTimeMicros = micros(); + currentTime = currentTimeMicros / 1000; // Main loop at 200Hz (IMU + control) if ((currentTime-mainLoop) > 5) // about 200Hz (every 5ms) { - G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!! + //G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!! + G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001; // Microseconds!!! mainLoop = currentTime; + previousTimeMicros = currentTimeMicros; //IMU DCM Algorithm Read_adc_raw(); // Read sensors raw data @@ -268,16 +275,33 @@ void loop() Euler_angles(); // Read radio values (if new data is available) - if (APM_RC.GetState() == 1) // New radio frame? + if (APM_RC.GetState() == 1) { // New radio frame? +#if AIRFRAME == QUAD read_radio(); +#endif +#if AIRFRAME == HELI + heli_read_radio(); +#endif + } // Attitude control if(flightMode == STABLE_MODE) { // STABLE Mode gled_speed = 1200; - if (AP_mode == AP_NORMAL_MODE) // Normal mode + if (AP_mode == AP_NORMAL_MODE) { // Normal mode +#if AIRFRAME == QUAD Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw); - else // Automatic mode : GPS position hold mode +#endif +#if AIRFRAME == HELI + heli_attitude_control(command_rx_roll,command_rx_pitch,command_rx_collective,command_rx_yaw); +#endif + }else{ // Automatic mode : GPS position hold mode +#if AIRFRAME == QUAD Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw); +#endif +#if AIRFRAME == HELI + heli_attitude_control(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_collective,command_rx_yaw); +#endif + } } else { // ACRO Mode gled_speed = 400; @@ -287,7 +311,9 @@ void loop() } // Send output commands to motor ESCs... +#if AIRFRAME == QUAD // we update the heli swashplate at about 60hz motor_output(); +#endif #ifdef IsCAM // Do we have cameras stabilization connected and in use? @@ -355,6 +381,11 @@ void loop() #ifdef IsGPS GPS.Read(); // Read GPS data #endif + +#if AIRFRAME == HELI + // Send output commands to heli swashplate... + heli_moveSwashPlate(); +#endif // Each of the six cases executes at 10Hz switch (medium_loopCounter){ case 0: // Magnetometer reading (10Hz) diff --git a/ArducopterNG/GCS.pde b/ArducopterNG/GCS.pde index 559ea75848..efd34b96dc 100644 --- a/ArducopterNG/GCS.pde +++ b/ArducopterNG/GCS.pde @@ -134,6 +134,9 @@ void readSerialCommand() { break; case 'Y': // Initialize EEPROM with default values defaultUserConfig(); +#if AIRFRAME == HELI + heli_defaultUserConfig(); +#endif break; case '1': // Receive transmitter calibration values ch_roll_slope = readFloatSerial(); @@ -490,4 +493,4 @@ float readFloatSerial() { return atof(data); } - + diff --git a/ArducopterNG/Heli.h b/ArducopterNG/Heli.h new file mode 100644 index 0000000000..de69e4e9fa --- /dev/null +++ b/ArducopterNG/Heli.h @@ -0,0 +1,167 @@ +/* + www.ArduCopter.com - www.DIYDrones.com + Copyright (c) 2010. All rights reserved. + An Open Source Arduino based multicopter. + + File : Arducopter.h + Version : v1.0, Aug 27, 2010 + Author(s): ArduCopter Team + Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, + Jani Hirvinen, Ken McEwans, Roberto Navoni, + Sandro Benigno, Chris Anderson + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ + +#ifndef HELI_H +#define HELI_H + +#include +#include "WProgram.h" +#include +#include // added by Randy +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega RC Library +#include // ArduPilot Mega Compass Library +#include // ArduPilot Mega DataFlash Library. +#include "../AP_Math/AP_Math.h" + +/**********************************************************************/ +// Channel definitions +#define CHANNEL_FRONT_LEFT 0 +#define CHANNEL_FRONT_RIGHT 1 +#define CHANNEL_REAR 2 +#define CHANNEL_YAW 3 + +/**********************************************************************/ +// EEPROM locations +#define EEPROM_BASE_ADDRESS 300 +#define EEPROM_MAGIC_NUMBER_ADDR EEPROM_BASE_ADDRESS +#define FRONT_LEFT_CCPM_MIN_ADDR EEPROM_BASE_ADDRESS+4 +#define FRONT_LEFT_CCPM_MAX_ADDR EEPROM_BASE_ADDRESS+8 +#define FRONT_RIGHT_CCPM_MIN_ADDR EEPROM_BASE_ADDRESS+12 +#define FRONT_RIGHT_CCPM_MAX_ADDR EEPROM_BASE_ADDRESS+16 +#define REAR_CCPM_MIN_ADDR EEPROM_BASE_ADDRESS+20 +#define REAR_CCPM_MAX_ADDR EEPROM_BASE_ADDRESS+24 +#define YAW_MIN_ADDR EEPROM_BASE_ADDRESS+28 +#define YAW_MAX_ADDR EEPROM_BASE_ADDRESS+32 +#define THROTTLE_MIN_ADDR EEPROM_BASE_ADDRESS+36 +#define THROTTLE_MAX_ADDR EEPROM_BASE_ADDRESS+40 + +#define EEPROM_MAGIC_NUMBER 12345.0 + +#define YAW_MODE_HEADING_HOLD 0 +#define YAW_MODE_RATE 1 + +#define HELI_STICK_TO_ANGLE_FACTOR 2.0 // To convert ccpm values (-50 ~ 50 ) to absolute angles. larger number means less lean +#define HELI_YAW_STICK_TO_ANGLE_FACTOR 0.5 // convert yaw (-50 ~ 50) to turn rate in degrees per second. larger number means slower turn rate + +// roll and pitch adjustment - attitude close to what's required to keep heli in one place when hovering. This will be added to the roll/pitch commands from the pilot +#define HELI_ADJUST_ROLL 4 +#define HELI_ADJUST_PITCH -3 + +// CCPM Types +#define HELI_CCPM_120_TWO_FRONT_ONE_BACK 0 +#define HELI_CCPM_120_ONE_FRONT_TWO_BACK 1 + +// define which CCPM we have +#define HELI_CCPM HELI_CCPM_120_TWO_FRONT_ONE_BACK + +// define DeAllocation matrix(converts radio inputs to roll, pitch and collective +// for example roll = (inputCh0*Row1Col1) + (inputCh1*Row1Col2) + (inputCh2*Row1Col3) +// pitch = (inputCh0*Row2Col1) + (inputCh1*Row2Col2) + (inputCh2*Row2Col3) +// collective = (inputCh0*Row3Col1) + (inputCh1*Row3Col2) + (inputCh2*Row3Col3) +// and Allocation matrix (converts roll, pitch, collective to servo outputs) +// for example servo0 = (roll*Row1Col1) + (pitch*Row1Col2) + (collective*Row1Col3) +// servo1 = (roll*Row2Col1) + (pitch*Row2Col2) + (collective*Row2Col3) +// servo2 = (roll*Row3Col1) + (pitch*Row3Col2) + (collective*Row3Col3) +#if HELI_CCPM == HELI_CCPM_120_TWO_FRONT_ONE_BACK + #define CCPM_DEALLOCATION 0.5774, -0.5774, 0.0000, \ + 0.3333, 0.3333, -0.6667, \ + 0.3333, 0.3333, 0.3333 + #define CCPM_ALLOCATION 0.8660,0.5000, 1.0000, \ + -0.8660, 0.5000, 1.0000, \ + 0.0000, -1.0000, 1.0000 +#endif + +#if HELI_CCPM == HELI_CCPM_120_ONE_FRONT_TWO_BACK + #define CCPM_DEALLOCATION 0.5774, -0.5774, 0.0000, \ + -0.3333,-0.3333, 0.6667, \ + 0.3333, 0.3333, 0.3333 + #define CCPM_ALLOCATION 0.8660, -0.5000, 1.0000, \ + -0.8660, -0.5000, 1.0000, \ + 0.0000, 1.0000, 1.0000 +#endif + +const Matrix3f ccpmDeallocation(CCPM_DEALLOCATION); +const Matrix3f ccpmAllocation(CCPM_ALLOCATION); + +/**********************************************************************/ +// time variables - we run at a different hertz than quads +unsigned long heli_previousTimeMicros = 0; + +// PWM Input Processing - Variable Definitions +float frontLeftCCPMmin; +float frontLeftCCPMmax; +float frontLeftCCPMslope; +float frontLeftCCPMintercept; + +float frontRightCCPMmin; +float frontRightCCPMmax; +float frontRightCCPMslope; +float frontRightCCPMintercept; + +float rearCCPMmin; +float rearCCPMmax; +float rearCCPMslope; +float rearCCPMintercept; + +float yawMin; +float yawMax; +float yawSlope; +float yawIntercept; + +Vector3f ccpmPercents; // Array of ccpm input values, converted to percents +Vector3f rollPitchCollPercent; // Array containing deallocated roll, pitch and collective percent commands +float ch_collective; +int collective_mid; +float control_collective; +float command_rx_collective; +float yawPercent; +float targetHeading; + +/// for sending values out to servos +Vector3f ccpmPercents_out; // Array of ccpm input values, converted to percents +Vector3f pitchRollCollPercent_out; // Array containing deallocated pitch, roll, and collective percent commands + +// heli debug +int heli_debug = 0; + +/**********************************************************************/ +// Output to Servos +int leftOut; +int rightOut; +int rearOut; +int yawOut; + +#endif HELI_H diff --git a/ArducopterNG/Heli.pde b/ArducopterNG/Heli.pde new file mode 100644 index 0000000000..4c2787b6b9 --- /dev/null +++ b/ArducopterNG/Heli.pde @@ -0,0 +1,333 @@ +/* + www.ArduCopter.com - www.DIYDrones.com + Copyright (c) 2010. All rights reserved. + An Open Source Arduino based multicopter. + + File : Heli.pde + Desc : code specific to traditional helicopters + Version : v1.0, Aug 27, 2010 + Author(s): ArduCopter Team + Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, + Jani Hirvinen, Ken McEwans, Roberto Navoni, + Sandro Benigno, Chris Anderson + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ + +#if AIRFRAME == HELI + +/**********************************************************************/ +// heli_readUserConfig - reads values in from EEPROM +void heli_readUserConfig() +{ + float magicNum = 0; + magicNum = readEEPROM(EEPROM_MAGIC_NUMBER_ADDR); + if( magicNum != EEPROM_MAGIC_NUMBER ) { + SerPri("No heli settings found in EEPROM. Using defaults"); + heli_defaultUserConfig(); + }else{ + frontLeftCCPMmin = readEEPROM(FRONT_LEFT_CCPM_MIN_ADDR); + frontLeftCCPMmax = readEEPROM(FRONT_LEFT_CCPM_MAX_ADDR); + frontRightCCPMmin = readEEPROM(FRONT_RIGHT_CCPM_MIN_ADDR); + frontRightCCPMmax = readEEPROM(FRONT_RIGHT_CCPM_MAX_ADDR); + rearCCPMmin = readEEPROM(REAR_CCPM_MIN_ADDR); + rearCCPMmax = readEEPROM(REAR_CCPM_MAX_ADDR); + yawMin = readEEPROM(YAW_MIN_ADDR); + yawMax = readEEPROM(YAW_MAX_ADDR); + } +} + +/**********************************************************************/ +// default the heli specific values to defaults +void heli_defaultUserConfig() +{ + // default CCPM values. + frontLeftCCPMmin = 1200; + frontLeftCCPMmax = 1800; + frontRightCCPMmin = 1900; + frontRightCCPMmax = 1100; + rearCCPMmin = 1200; + rearCCPMmax = 1800; + yawMin = 1200; + yawMax = 1800; + + // default PID values - Roll + KP_QUAD_ROLL = 1.000; + KI_QUAD_ROLL = 0.150; + STABLE_MODE_KP_RATE_ROLL = 0.000; + + // default PID values - Pitch + KP_QUAD_PITCH = 1.200; + KI_QUAD_PITCH = 0.120; + STABLE_MODE_KP_RATE_PITCH = 0.000; + + // default PID values - Yaw + Kp_RateYaw = 2.000; // heading P term + Ki_RateYaw = 0.100; // heading I term + KP_QUAD_YAW = 0.150; // yaw rate P term + KI_QUAD_YAW = 0.030; // yaw rate I term + STABLE_MODE_KP_RATE_YAW = 0.000; // not used +} + +/**********************************************************************/ +// displaySettings - displays heli specific user settings +void heli_displaySettings() +{ + SerPri("frontLeftCCPM min: "); + SerPri(frontLeftCCPMmin); + SerPri("\tmax:"); + SerPri(frontLeftCCPMmax); + + if( abs(frontLeftCCPMmin-frontLeftCCPMmax)<50 || frontLeftCCPMmin < 900 || frontLeftCCPMmin > 2100 || frontLeftCCPMmax < 900 || frontLeftCCPMmax > 2100 ) + SerPrln("\t\t<-- check"); + else + SerPrln(); + + SerPri("frontRightCCPM min: "); + SerPri(frontRightCCPMmin); + SerPri("\tmax:"); + SerPri(frontRightCCPMmax); + if( abs(frontRightCCPMmin-frontRightCCPMmax)<50 || frontRightCCPMmin < 900 || frontRightCCPMmin > 2100 || frontRightCCPMmax < 900 || frontRightCCPMmax > 2100 ) + SerPrln("\t\t<-- check"); + else + SerPrln(); + + SerPri("rearCCPM min: "); + SerPri(rearCCPMmin); + SerPri("\tmax:"); + SerPri(rearCCPMmax); + if( abs(rearCCPMmin-rearCCPMmax)<50 || rearCCPMmin < 900 || rearCCPMmin > 2100 || rearCCPMmax < 900 || rearCCPMmax > 2100 ) + SerPrln("\t\t<-- check"); + else + SerPrln(); + + SerPri("yaw min: "); + SerPri(yawMin); + SerPri("\tmax:"); + SerPri(yawMax); + if( abs(yawMin-yawMax)<50 || yawMin < 900 || yawMin > 2100 || yawMax < 900 || yawMax > 2100 ) + SerPrln("\t\t<-- check"); + else + SerPrln(); + + SerPrln(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Setup Procedure +//////////////////////////////////////////////////////////////////////////////// +void heli_setup() +{ + // read heli specific settings (like CCPM values) from EEPROM + heli_readUserConfig(); + + // update CCPM values + frontLeftCCPMslope = 100 / (frontLeftCCPMmax - frontLeftCCPMmin); + frontLeftCCPMintercept = 100 - (frontLeftCCPMslope * frontLeftCCPMmax); + frontRightCCPMslope = 100 / (frontRightCCPMmax - frontRightCCPMmin); + frontRightCCPMintercept = 100 - (frontRightCCPMslope * frontRightCCPMmax); + rearCCPMslope = 100 / (rearCCPMmax - rearCCPMmin); + rearCCPMintercept = 100 - (rearCCPMslope * rearCCPMmax); + yawSlope = 100 / (yawMax - yawMin); + yawIntercept = 50 - (yawSlope * yawMax); + + // hardcode mids because we will use ccpm + roll_mid = ROLL_MID; + pitch_mid = PITCH_MID; + collective_mid = 1500; + yaw_mid = (yawMin+yawMax)/2; +} + +/**********************************************************************/ +// Radio decoding +void heli_read_radio() +{ + static int count = 0; + // left channel + ccpmPercents.x = frontLeftCCPMslope * APM_RC.InputCh(CHANNEL_FRONT_LEFT) + frontLeftCCPMintercept; + + // right channel + ccpmPercents.y = frontRightCCPMslope * APM_RC.InputCh(CHANNEL_FRONT_RIGHT) + frontRightCCPMintercept; + + // rear channel + ccpmPercents.z = rearCCPMslope * APM_RC.InputCh(CHANNEL_REAR) + rearCCPMintercept; + + // decode the ccpm + rollPitchCollPercent = ccpmDeallocation * ccpmPercents; + + // get the yaw (not coded) + yawPercent = yawSlope * APM_RC.InputCh(CHANNEL_YAW) + yawIntercept; + + // we add 1500 to make it fit in with rest of arduCopter code.. + ch_roll = rollPitchCollPercent.x; + ch_pitch = rollPitchCollPercent.y; + ch_collective = rollPitchCollPercent.z; + + // allow a bit of a dead zone for the yaw + if( abs(yawPercent) < 2 ) + ch_yaw = 0; + else + ch_yaw = yawPercent; + + // convert to absolute angles + command_rx_roll = ch_roll / HELI_STICK_TO_ANGLE_FACTOR + HELI_ADJUST_ROLL; // Convert stick position to absolute angles + command_rx_pitch = ch_pitch / HELI_STICK_TO_ANGLE_FACTOR + HELI_ADJUST_PITCH; // Convert stick position to absolute angles + command_rx_collective = ch_collective; + command_rx_yaw = ch_yaw / HELI_YAW_STICK_TO_ANGLE_FACTOR; // Convert stick position to turn rate + + // hardcode flight mode + flightMode = STABLE_MODE; +} + +/**********************************************************************/ +// output to swash plate based on control variables +// Uses these global variables: +// control_roll : -50 ~ 50 +// control_pitch : -50 ~ 50 +// control_collective : 0 ~ 100 +// control_yaw : -50 ~ 50 +void heli_moveSwashPlate() +{ + static int count = 0; + // turn pitch, roll, collective commands into ccpm values (i.e. values for each servo) + ccpmPercents_out = ccpmAllocation * Vector3f(control_roll, control_pitch, control_collective); + + // calculate values to be sent out to RC Output channels + leftOut = (ccpmPercents_out.x - frontLeftCCPMintercept) / frontLeftCCPMslope; + rightOut = (ccpmPercents_out.y - frontRightCCPMintercept) / frontRightCCPMslope; + rearOut = (ccpmPercents_out.z - rearCCPMintercept) / rearCCPMslope; + yawOut = (control_yaw - yawIntercept) / yawSlope; + + APM_RC.OutputCh(CHANNEL_FRONT_LEFT,leftOut); + APM_RC.OutputCh(CHANNEL_FRONT_RIGHT,rightOut); + APM_RC.OutputCh(CHANNEL_REAR,rearOut); + APM_RC.OutputCh(CHANNEL_YAW,yawOut); + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); +} + +/**********************************************************************/ +// ROLL, PITCH and YAW PID controls... +// Input : desired Roll, Pitch absolute angles +// collective as a percentage from 0~100 +// yaw as a rate of rotation +// Output : control_roll - roll servo as a percentage (-50 to 50) +// control_pitch - pitch servo as a percentage (-50 to 50) +// control_collective - collective servo as a percentage (0 to 100) +// control_yaw - yaw servo as a percentage (0 to 100) +void heli_attitude_control(int command_roll, int command_pitch, int command_collective, int command_yaw) +{ + static float firstIteration = 1; + static float command_yaw_previous = 0; + static float previousYawRate = 0; + float stable_roll, stable_pitch; + float currentYawRate; + float control_yaw_rate; + float err_heading; + static int aCounter = 0; + float heli_G_Dt; + + // get current time + heli_G_Dt = (currentTimeMicros-heli_previousTimeMicros) * 0.000001; // Microseconds!!! + heli_previousTimeMicros = currentTimeMicros; + + // always pass through collective command + control_collective = command_rx_collective; + + // ROLL CONTROL -- TWO PIDS - ANGLE CONTROL + RATE CONTROL + // P term + err_roll = command_roll - ToDeg(roll); + err_roll = constrain(err_roll,-25,25); // to limit max roll command... + // I term + roll_I += err_roll*heli_G_Dt*KI_QUAD_ROLL; + roll_I = constrain(roll_I,-10,10); + // D term + roll_D = ToDeg(Omega[0]) * STABLE_MODE_KP_RATE_ROLL; // Take into account Angular velocity + roll_D = constrain(roll_D,-25,25); + + // PID control + control_roll = KP_QUAD_ROLL*err_roll + roll_I + roll_D; + control_roll = constrain(control_roll,-50,50); + + // PITCH CONTROL -- TWO PIDS - ANGLE CONTROL + RATE CONTROL + // P term + err_pitch = command_pitch - ToDeg(pitch); + err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command... + // I term + pitch_I += err_pitch * heli_G_Dt * KI_QUAD_PITCH; + pitch_I = constrain(pitch_I,-10,10); + // D term + pitch_D = ToDeg(Omega[1]) * STABLE_MODE_KP_RATE_PITCH; // Take into account Angular velocity + pitch_D = constrain(pitch_D,-25,25); + // PID control + control_pitch = KP_QUAD_PITCH*err_pitch + pitch_I + pitch_D; + control_pitch = constrain(control_pitch,-50,50); + + + // YAW CONTROL + if( command_yaw == 0 ) // heading hold mode + { + // check we don't need to reset targetHeading + if( command_yaw_previous != 0 ) + targetHeading = ToDeg(yaw); + + // ensure reasonable targetHeading + if( firstIteration || targetHeading > 180 || targetHeading < -180 ) + { + firstIteration = 0; + targetHeading = ToDeg(yaw); + } + + err_heading = Normalize_angle(targetHeading - ToDeg(yaw)); + err_heading = constrain(err_heading,-90,90); // don't make it travel any faster beyond 90 degrees + + heading_I += err_heading * heli_G_Dt * Ki_RateYaw; + heading_I = constrain(heading_I,-20,20); + + // PID control - a bit bad - we're using the acro mode's PID values because there's not PID for heading + control_yaw_rate = Kp_RateYaw*err_heading + heading_I; + control_yaw_rate = constrain(control_yaw_rate,-100,100); // to limit max yaw command + }else{ // rate mode + err_heading = 0; + control_yaw_rate = command_yaw; + } + command_yaw_previous = command_yaw; + + // YAW RATE CONTROL + currentYawRate = ToDeg(Gyro_Scaled_Z(read_adc(2))); + //currentYawRate = ToDeg(Omega_Vector[2]); <-- makes things very unstable!! + err_yaw = control_yaw_rate-currentYawRate; + + yaw_I += err_yaw * heli_G_Dt * KI_QUAD_YAW; + yaw_I = constrain(yaw_I, -20, 20); + + //yaw_D = currentYawRate - previousYawRate; + previousYawRate = currentYawRate; + + // PID control + control_yaw = (KP_QUAD_YAW*err_yaw + yaw_I); + control_yaw = constrain(control_yaw,-50,50); +} + +#endif // #if AIRFRAME == HELI diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde index c9469a841a..505867e9b7 100644 --- a/ArducopterNG/Navigation.pde +++ b/ArducopterNG/Navigation.pde @@ -55,6 +55,7 @@ void read_GPS_data() /* GPS based Position control */ void Position_control(long lat_dest, long lon_dest) { +#ifdef IsGPS long Lon_diff; long Lat_diff; @@ -93,6 +94,7 @@ void Position_control(long lat_dest, long lon_dest) command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command //Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10); +#endif } void Reset_I_terms_navigation() diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index 73876770f5..e8450056a4 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -58,12 +58,21 @@ void APM_Init() { APM_RC.Init(); // APM Radio initialization +#if AIRFRAME == QUAD // 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); +#endif +#if AIRFRAME == HELI + // RC channels Initialization (heli servos) + APM_RC.OutputCh(0,CHANN_CENTER); // mid position + APM_RC.OutputCh(1,CHANN_CENTER); + APM_RC.OutputCh(2,CHANN_CENTER); + APM_RC.OutputCh(3,CHANN_CENTER); +#endif // Make sure that Relay is switched off. digitalWrite(RELAY,LOW); @@ -119,11 +128,21 @@ void APM_Init() { if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500; if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500; +#if AIRFRAME == QUAD // 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); +#endif + +#if AIRFRAME == HELI + // RC channels Initialization (heli servos) + APM_RC.OutputCh(0,CHANN_CENTER); // mid position + APM_RC.OutputCh(1,CHANN_CENTER); + APM_RC.OutputCh(2,CHANN_CENTER); + APM_RC.OutputCh(3,CHANN_CENTER); +#endif // Initialise Wire library used by Magnetometer and Barometer Wire.begin(); @@ -181,6 +200,11 @@ void APM_Init() { //timer = millis(); //tlmTimer = millis(); + // initialise helicopter +#if AIRFRAME == HELI + heli_setup(); +#endif + #ifdef IsAM // Switch Left & Right lights on digitalWrite(RI_LED, HIGH);