ardupilot/Tools/APM_radio_test/APM_radio_test.pde

77 lines
1.5 KiB
Plaintext
Raw Normal View History

2011-09-08 22:31:32 -03:00
#include <avr/io.h>
2011-12-22 17:17:04 -04:00
#include <Arduino_Mega_ISR_Registry.h>
2011-09-08 22:31:32 -03:00
#include <APM_RC.h> // ArduPilot Mega RC Library
/*
ArduPilotMega radio test tool
Authors: Doug Weibel, Jose Julio
*/
// GENERAL VARIABLE DECLARATIONS
// --------------------------------------------
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
4 Rudder (if we have ailerons)
5 TBD
6 TBD
7 TBD
8 Mode
*/
int radio_in[8]; // current values from the transmitter - microseconds
float servo_out[] = {0,0,0,0,0,0,0};
unsigned long fast_loopTimer = 0; // current values to the servos - -45 to 45 degrees, except [3] is 0 to 100
2011-12-22 17:17:04 -04:00
Arduino_Mega_ISR_Registry isr_registry;
APM_RC_APM1 APM_RC;
2011-09-08 22:31:32 -03:00
void setup() {
Serial.begin(115200);
2011-12-22 17:17:04 -04:00
isr_registry.init();
APM_RC.Init(&isr_registry); // APM Radio initialization
2011-09-08 22:31:32 -03:00
}
void loop()
{
// -----------------------------------------------------------------
if (millis()-fast_loopTimer > 199) {
fast_loopTimer = millis();
read_radio();
Serial.print("ch1: ");
Serial.print(radio_in[0]);
Serial.print(" ch2: ");
Serial.print(radio_in[1]);
Serial.print(" ch3: ");
Serial.print(radio_in[2]);
Serial.print(" ch4: ");
Serial.print(radio_in[3]);
Serial.print(" ch5: ");
Serial.print(radio_in[4]);
Serial.print(" ch6: ");
Serial.print(radio_in[5]);
Serial.print(" ch7: ");
Serial.print(radio_in[6]);
Serial.print(" ch8: ");
Serial.println(radio_in[7]);
}
}
void read_radio()
{
for (int y=0;y<8;y++) radio_in[y] = APM_RC.InputCh(y);
2014-04-15 03:50:00 -03:00
}