Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Adjusted IMU gains and Accelerometers dynamic weighting 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@19 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-06-21 20:15:22 +00:00
parent 015b2599c6
commit 830fa2b104
6 changed files with 297 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
ArduCopter.pde
View File

@ -5,10 +5,11 @@
/* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */
/* Ardupilot core code : from DIYDrones.com development team */
/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */
/* Authors : Jose Julio, Ted Carancho, Jordi Muñoz */
/* Date : 27-05-2010 */
/* Version : 1.0 beta */
/* Hardware : ArduPilot Mega + Sensor Shield (codename:FOXTRAP2) */
/* Authors : Jose Julio, Ted Carancho (aeroquad), Jordi Muñoz, */
/* Roberto Navoni, ... (Arcucopter team) */
/* Date : 17-06-2010 */
/* Version : 1.1 beta */
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
/* This code use this libraries : */
/* APM_RC_QUAD : Radio library (adapted for quads) */
/* APM_ADC : External ADC library */
@ -32,7 +33,7 @@
#define LED_Green 37
#define RELE_pin 47
#define SW1_pin 41
#define SW2_pin 42
#define SW2_pin 40
/* *** */
/* ***************************************************************************** */
@ -45,46 +46,46 @@
#define MAGNETOMETER 1 // 0 : No magnetometer, 1: Magnetometer
// QuadCopter Attitude control PID GAINS
#define KP_QUAD_ROLL 1.7 // 1.5 //1.75
#define KD_QUAD_ROLL 0.45 //0.35 // 0.4 //Absolute max:0.85
#define KI_QUAD_ROLL 0.4 // 0.4 //0.45
#define KP_QUAD_PITCH 1.7
#define KD_QUAD_PITCH 0.45
#define KI_QUAD_PITCH 0.4
#define KP_QUAD_YAW 3.5 // 3.8
#define KP_QUAD_ROLL 1.8 // 1.5 //1.75
#define KD_QUAD_ROLL 0.48 //0.35 // 0.4 //Absolute max:0.85
#define KI_QUAD_ROLL 0.30 // 0.4 //0.45
#define KP_QUAD_PITCH 1.8
#define KD_QUAD_PITCH 0.48
#define KI_QUAD_PITCH 0.30 //0.4
#define KP_QUAD_YAW 3.6 // 3.8
#define KD_QUAD_YAW 1.2 // 1.3
#define KI_QUAD_YAW 0.15 // 0.15
#define KD_QUAD_COMMAND_PART 12.0 // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs
#define KD_QUAD_COMMAND_PART 2.0 // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs
// Position control PID GAINS
#define KP_GPS_ROLL 0.03
#define KD_GPS_ROLL 0.0
#define KI_GPS_ROLL 0.01
#define KP_GPS_PITCH 0.03
#define KD_GPS_PITCH 0.0
#define KI_GPS_PITCH 0.01
#define KP_GPS_ROLL 0.012
#define KD_GPS_ROLL 0.005
#define KI_GPS_ROLL 0.004
#define KP_GPS_PITCH 0.012
#define KD_GPS_PITCH 0.005
#define KI_GPS_PITCH 0.004
#define GPS_MAX_ANGLE 16 // Maximun command roll and pitch angle from position control
#define GPS_MAX_ANGLE 10 // Maximun command roll and pitch angle from position control
// Altitude control PID GAINS
#define KP_ALTITUDE 0.8
#define KD_ALTITUDE 0.3
#define KD_ALTITUDE 0.2
#define KI_ALTITUDE 0.2
// The IMU should be correctly adjusted : Gyro Gains and also initial IMU offsets:
// We have to take this values with the IMU flat (0º roll, 0º pitch)
#define acc_offset_x 2039
#define acc_offset_y 2035
#define acc_offset_z 2014 // We need to rotate the IMU exactly 90º to take this value
#define gyro_offset_roll 1650
#define gyro_offset_pitch 1690
#define gyro_offset_yaw 1690
#define acc_offset_x 2079
#define acc_offset_y 2050
#define acc_offset_z 2008 // We need to rotate the IMU exactly 90º to take this value
#define gyro_offset_roll 1659 //1650
#define gyro_offset_pitch 1618 //1690
#define gyro_offset_yaw 1673
// 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 : 414
#define GRAVITY 414 //this equivalent to 1G in the raw data coming from the accelerometer
// 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
@ -92,19 +93,17 @@
// 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_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
//#define Kp_ROLLPITCH 0.0125 //0.010 // Pitch&Roll Proportional Gain
#define Kp_ROLLPITCH 0.002 //0.003125
//#define Ki_ROLLPITCH 0.000010 // Pitch&Roll Integrator Gain
#define Ki_ROLLPITCH 0.0000025
#define Kp_ROLLPITCH 0.0032 //0.002 //0.003125 // Pitch&Roll Proportional Gain
#define Ki_ROLLPITCH 0.000001 //0.000005 //0.0000025 // Pitch&Roll Integrator Gain
#define Kp_YAW 1.5 // Yaw Porportional Gain
#define Ki_YAW 0.00005 // Yaw Integrator Gain
#define Ki_YAW 0.00005 //0.00005 // Yaw Integrator Gain
/*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
@ -159,27 +158,11 @@ float Temporary_Matrix[3][3]={
float speed_3d=0;
int GPS_ground_speed=0;
//Log variables
int log_roll;
int log_pitch;
int log_yaw;
long timer=0; //general porpuse timer
long timer_old;
// Sonar variables
static volatile unsigned long sonar_start_ms;
static volatile unsigned char sonar_start_t0;
static volatile unsigned long sonar_pulse_start_ms;
static volatile unsigned char sonar_pulse_start_t0;
static volatile unsigned long sonar_pulse_end_ms;
static volatile unsigned char sonar_pulse_end_t0;
static volatile byte sonar_status=0;
static volatile byte sonar_new_data=0;
int sonar_value=0;
// Attitude control variables
float command_rx_roll=0; // comandos recibidos rx
float command_rx_roll=0; // User commands
float command_rx_roll_old;
float command_rx_roll_diff;
float command_rx_pitch=0;
@ -187,12 +170,12 @@ float command_rx_pitch_old;
float command_rx_pitch_diff;
float command_rx_yaw=0;
float command_rx_yaw_diff;
int control_roll; // resultados del control
int control_roll; // PID control results
int control_pitch;
int control_yaw;
float K_aux;
// Attitude control
// Attitude PID controls
float roll_I=0;
float roll_D;
float err_roll;
@ -227,6 +210,10 @@ 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;
@ -249,7 +236,7 @@ int ch_yaw;
void Altitude_control(int target_sonar_altitude)
{
err_altitude_old = err_altitude;
err_altitude = target_sonar_altitude - sonar_value;
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);
@ -316,7 +303,7 @@ void Attitude_control()
roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command)
// PID control
control_roll = KP_QUAD_ROLL*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I;
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
@ -332,7 +319,7 @@ void Attitude_control()
pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]);
// PID control
control_pitch = KP_QUAD_PITCH*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I;
control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I;
// YAW CONTROL
@ -342,7 +329,7 @@ void Attitude_control()
else if(err_yaw < -180)
err_yaw += 360;
err_yaw = constrain(err_yaw,-25,25); // to limit max yaw command...
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);
@ -390,11 +377,14 @@ void setup()
pinMode(RELE_pin,OUTPUT); // Rele output
digitalWrite(RELE_pin,LOW);
delay(250);
APM_RC_QUAD.Init(); // APM Radio initialization
APM_ADC.Init(); // APM ADC library initialization
DataFlash.Init(); // DataFlash log initialization
GPS.Init(); // GPS Initialization
delay(100);
// RC channels Initialization (Quad motors)
APM_RC_QUAD.OutputCh(0,MIN_THROTTLE+15); // Motors stoped
APM_RC_QUAD.OutputCh(1,MIN_THROTTLE+15);
@ -498,6 +488,11 @@ void loop(){
int aux;
int i;
float aux_float;
//Log variables
int log_roll;
int log_pitch;
int log_yaw;
if((millis()-timer)>=14) // Main loop 70Hz
{
@ -533,8 +528,16 @@ void loop(){
Serial.print(",");
Serial.print(log_yaw);
/*
for (int i=0;i<6;i++)
{
Serial.print(AN[i]);
Serial.print(",");
}
*/
// Write Sensor raw data to DataFlash log
//Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
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);
@ -547,10 +550,10 @@ void loop(){
ch_throttle = channel_filter(APM_RC_QUAD.InputCh(2),ch_throttle);
ch_yaw = channel_filter(APM_RC_QUAD.InputCh(3),ch_yaw);
command_rx_roll_old = command_rx_roll;
command_rx_roll = (ch_roll-CHANN_CENTER)/15.0;
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)/15.0;
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;
@ -562,14 +565,12 @@ void loop(){
// 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 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 0.4)*0.2;
K_aux = K_aux*0.8 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 1.7)*0.2; // /300 + 1.0
if (K_aux < 0)
K_aux = 0;
/*
Serial.print(",");
Serial.print(K_aux);
*/
//Serial.print(",");
//Serial.print(K_aux);
// We read the Quad Mode from Channel 5
if (APM_RC_QUAD.InputCh(4) < 1200)

12
DCM.pde
View File

@ -7,7 +7,7 @@ void Read_adc_raw(void)
for (int i=0;i<6;i++)
AN[i] = APM_ADC.Ch(sensors[i]);
// Correction for non ratiometric sensor (test)
// Correction for non ratiometric sensor (test code)
//temp = APM_ADC.Ch(3);
//AN[0] += 1500-temp;
//AN[1] += 1500-temp;
@ -67,8 +67,8 @@ void Drift_correction(void)
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);
// Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0)
Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1);
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
@ -109,9 +109,9 @@ void Matrix_update(void)
//Accel_Vector[2]=read_adc(5); // acc z
// Low pass filter on accelerometer data (to filter vibrations)
Accel_Vector[0]=Accel_Vector[0]*0.4 + (float)read_adc(3)*0.6; // acc x
Accel_Vector[1]=Accel_Vector[1]*0.4 + (float)read_adc(4)*0.6; // acc y
Accel_Vector[2]=Accel_Vector[2]*0.4 + (float)read_adc(5)*0.6; // acc z
Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x
Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y
Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional

167
libraries/APM_RC/APM_RC.cpp Normal file
View File

@ -0,0 +1,167 @@
/*
APM_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
RC Input : PPM signal on IC4 pin
RC Output : 11 Servo outputs (standard 20ms frame)
Methods:
Init() : Initialization of interrupts an Timers
OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10
InputCh(ch) : Read a channel input value. ch=0..7
GetState() : Returns the state of the input. 1 => New radio frame to process
Automatically resets when we call InputCh to read channels
*/
#include "APM_RC.h"
#include <avr/interrupt.h>
#include "WProgram.h"
// Variable definition for Input Capture interrupt
volatile unsigned int ICR4_old;
volatile unsigned char PPM_Counter=0;
volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400};
volatile unsigned char radio_status=0;
/****************************************************
Input Capture Interrupt ICP4 => PPM signal read
****************************************************/
ISR(TIMER4_CAPT_vect)
{
unsigned int Pulse;
unsigned int Pulse_Width;
Pulse=ICR4;
if (Pulse<ICR4_old) // Take care of the overflow of Timer4 (TOP=40000)
Pulse_Width=(Pulse + 40000)-ICR4_old; //Calculating pulse
else
Pulse_Width=Pulse-ICR4_old; //Calculating pulse
if (Pulse_Width>8000) // SYNC pulse?
PPM_Counter=0;
else
{
PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7)
PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse.
if (PPM_Counter >= NUM_CHANNELS)
radio_status = 1;
}
ICR4_old = Pulse;
}
// Constructors ////////////////////////////////////////////////////////////////
APM_RC_Class::APM_RC_Class()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void APM_RC_Class::Init(void)
{
// Init PWM Timer 1
pinMode(11,OUTPUT); // (PB5/OC1A)
pinMode(12,OUTPUT); //OUT2 (PB6/OC1B)
pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
//Remember the registers not declared here remains zero by default...
TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1)|(1<<COM1C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all...
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11); //Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet
OCR1A = 3000; //PB5, none
OCR1B = 3000; //PB6, OUT2
OCR1C = 3000; //PB7 OUT3
ICR1 = 40000; //50hz freq...Datasheet says (system_freq/prescaler)/target frequency. So (16000000hz/8)/50hz=40000,
// Init PWM Timer 3
pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
pinMode(4,OUTPUT); // (PE3/OC3A)
TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
OCR3A = 3000; //PE3, NONE
OCR3B = 3000; //PE4, OUT7
OCR3C = 3000; //PE5, OUT6
ICR3 = 40000; //50hz freq
// Init PWM Timer 5
pinMode(44,OUTPUT); //OUT1 (PL5/OC5C)
pinMode(45,OUTPUT); //OUT0 (PL4/OC5B)
pinMode(46,OUTPUT); // (PL3/OC5A)
TCCR5A =((1<<WGM51)|(1<<COM5A1)|(1<<COM5B1)|(1<<COM5C1));
TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
OCR5A = 3000; //PL3,
OCR5B = 3000; //PL4, OUT0
OCR5C = 3000; //PL5, OUT1
ICR5 = 40000; //50hz freq
// Init PPM input and PWM Timer 4
pinMode(49, INPUT); // ICP4 pin (PL0) (PPM input)
pinMode(7,OUTPUT); //OUT5 (PH4/OC4B)
pinMode(8,OUTPUT); //OUT4 (PH5/OC4C)
TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
//Prescaler set to 8, that give us a resolution of 0.5us
// Input Capture rising edge
TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
OCR4A = 40000; ///50hz freq.
OCR4B = 3000; //PH4, OUT5
OCR4C = 3000; //PH5, OUT4
//TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge).
//TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
}
void APM_RC_Class::OutputCh(unsigned char ch, int pwm)
{
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
pwm<<=1; // pwm*2;
switch(ch)
{
case 0: OCR5B=pwm; break; //ch0
case 1: OCR5C=pwm; break; //ch1
case 2: OCR1B=pwm; break; //ch2
case 3: OCR1C=pwm; break; //ch3
case 4: OCR4C=pwm; break; //ch4
case 5: OCR4B=pwm; break; //ch5
case 6: OCR3C=pwm; break; //ch6
case 7: OCR3B=pwm; break; //ch7
case 8: OCR5A=pwm; break; //ch8, PL3
case 9: OCR1A=pwm; break; //ch9, PB5
case 10: OCR3A=pwm; break; //ch10, PE3
}
}
int APM_RC_Class::InputCh(unsigned char ch)
{
int result;
int result2;
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
result = PWM_RAW[ch]>>1; // Because timer runs at 0.5us we need to do value/2
result2 = PWM_RAW[ch]>>1;
if (result != result2)
result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine)
// Limit values to a valid range
result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
radio_status=0; // Radio channel read
return(result);
}
unsigned char APM_RC_Class::GetState(void)
{
return(radio_status);
}
// make one instance for the user to use
APM_RC_Class APM_RC;

21
libraries/APM_RC/APM_RC.h Normal file
View File

@ -0,0 +1,21 @@
#ifndef APM_RC_h
#define APM_RC_h
#define NUM_CHANNELS 8
#define MIN_PULSEWIDTH 900
#define MAX_PULSEWIDTH 2100
class APM_RC_Class
{
private:
public:
APM_RC_Class();
void Init();
void OutputCh(unsigned char ch, int pwm);
int InputCh(unsigned char ch);
unsigned char GetState();
};
extern APM_RC_Class APM_RC;
#endif

31
libraries/APM_RC/examples/APM_radio/APM_radio.pde Normal file
View File

@ -0,0 +1,31 @@
/*
Example of APM_RC library.
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
Print Input values and send Output to the servos
(Works with last PPM_encoder firmware)
*/
#include <APM_RC.h> // ArduPilot Mega RC Library
void setup()
{
APM_RC.Init(); // APM Radio initialization
Serial.begin(57600);
Serial.println("ArduPilot Mega RC library test");
delay(1000);
}
void loop()
{
if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20)
{
Serial.print("CH:");
for(int i=0;i<8;i++)
{
Serial.print(APM_RC.InputCh(i)); // Print channel values
Serial.print(",");
APM_RC.OutputCh(i,APM_RC.InputCh(i)); // Copy input to Servos
}
Serial.println();
}
}

5
libraries/APM_RC/keywords.txt Normal file
View File

@ -0,0 +1,5 @@
APM_RC KEYWORD1
begin KEYWORD2
InputCh KEYWORD2
OutputCh KEYWORD2
GetState KEYWORD2