aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopterNG: added TraditionalHeli support, changed G_dt to work in microseconds (Issue #37),

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@909 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-11-24 01:30:41 +00:00
parent 2eb714aa51
commit 715cd4d3b0
8 changed files with 576 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArducopterNG/ArduUser.h
View File

@ -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

1
ArducopterNG/Arducopter.h
View File

@ -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;

43
ArducopterNG/ArducopterNG.pde
View File

@ -182,6 +182,9 @@
//#include <GPS_NMEA.h> // 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)

5
ArducopterNG/GCS.pde
View File

@ -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);
}

167
ArducopterNG/Heli.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
#ifndef HELI_H
#define HELI_H
#include <avr/interrupt.h>
#include "WProgram.h"
#include <Wire.h>
#include <EEPROM.h> // added by Randy
#include <APM_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <APM_Compass.h> // ArduPilot Mega Compass Library
#include <DataFlash.h> // 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

333
ArducopterNG/Heli.pde Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
* ************************************************************** *
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

2
ArducopterNG/Navigation.pde
View File

@ -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()

24
ArducopterNG/System.pde
View File

@ -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);