mirror of https://github.com/ArduPilot/ardupilot
Ardupilot Hardware RC Library for the 328
git-svn-id: https://arducopter.googlecode.com/svn/trunk@280 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
6215ad2471
commit
61179ba90f
|
@ -1,31 +0,0 @@
|
|||
/*
|
||||
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();
|
||||
}
|
||||
}
|
|
@ -1,48 +0,0 @@
|
|||
/*
|
||||
Example of AP_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 <AP_RC.h> // ArduPilot Mega RC Library
|
||||
AP_RC rc;
|
||||
|
||||
#define CH_ROLL 0
|
||||
#define CH_PITCH 1
|
||||
#define CH_THROTTLE 2
|
||||
#define CH_RUDDER 3
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.println("ArduPilot RC library test");
|
||||
|
||||
int trims[] = {1500,1500,1200,1500};
|
||||
rc.init(trims);
|
||||
|
||||
delay(1000);
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
delay(20);
|
||||
rc.read_pwm();
|
||||
for(int y=0; y<4; y++) {
|
||||
rc.set_ch_pwm(y, rc.input[y]); // send to Servos
|
||||
}
|
||||
print_pwm();
|
||||
}
|
||||
|
||||
|
||||
void print_pwm()
|
||||
{
|
||||
Serial.print("ch1 ");
|
||||
Serial.print(rc.input[CH_ROLL],DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(rc.input[CH_PITCH],DEC);
|
||||
Serial.print("\tch3 :");
|
||||
Serial.print(rc.input[CH_THROTTLE],DEC);
|
||||
Serial.print("\tch4 :");
|
||||
Serial.println(rc.input[CH_RUDDER],DEC);
|
||||
}
|
Loading…
Reference in New Issue