mirror of https://github.com/ArduPilot/ardupilot
105 lines
2.6 KiB
Plaintext
105 lines
2.6 KiB
Plaintext
/*
|
|
Example of RC_Channel library.
|
|
Code by James Goppert/ Jason Short. 2010
|
|
DIYDrones.com
|
|
|
|
*/
|
|
|
|
#include <FastSerial.h>
|
|
#include <AP_Common.h>
|
|
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
|
|
#include <AP_EEProm.h>
|
|
#include <APM_RC.h>
|
|
|
|
FastSerialPort0(Serial); // make sure this procees variable declarations
|
|
|
|
// test settings
|
|
uint8_t nChannels = 8;
|
|
bool loadEEProm = false;
|
|
bool saveEEProm = false;
|
|
|
|
// channel configuration
|
|
Vector< AP_EEPromVar<float> * > scale;
|
|
Vector< AP_EEPromVar<uint16_t> * > pwmMin;
|
|
Vector< AP_EEPromVar<uint16_t> * > pwmNeutral;
|
|
Vector< AP_EEPromVar<uint16_t> * > pwmMax;
|
|
Vector< AP_EEPromVar<uint16_t> * > pwmDeadZone;
|
|
Vector< AP_Var<bool> * > filter;
|
|
Vector< AP_Var<bool> * > reverse;
|
|
Vector< AP_RcChannel * > rc;
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
Serial.println("ArduPilot RC Channel test");
|
|
APM_RC.Init(); // APM Radio initialization
|
|
|
|
// add channels
|
|
for (int i=0;i<nChannels;i++)
|
|
{
|
|
char num[5];
|
|
itoa(i+1,num,10);
|
|
|
|
Serial.printf("\nInitializing channel %d", i+1);
|
|
|
|
// initialize eeprom settings
|
|
scale.push_back(new AP_EEPromVar<float>(1.0,strcat("SCALE",num)));
|
|
pwmMin.push_back(new AP_EEPromVar<uint16_t>(1200,strcat("PWM_MIN",num)));
|
|
pwmNeutral.push_back(new AP_EEPromVar<uint16_t>(1500,strcat("PWM_NEUTRAL",num)));
|
|
pwmMax.push_back(new AP_EEPromVar<uint16_t>(1800,strcat("PWM_MAX",num)));
|
|
pwmDeadZone.push_back(new AP_EEPromVar<uint16_t>(10,strcat("PWM_DEADZONE",num)));
|
|
filter.push_back(new AP_EEPromVar<bool>(false,strcat("FILTER",num)));
|
|
reverse.push_back(new AP_EEPromVar<bool>(false,strcat("REVERSE",num)));
|
|
|
|
// save
|
|
if (saveEEProm)
|
|
{
|
|
scale[i]->save();
|
|
pwmMin[i]->save();
|
|
pwmNeutral[i]->save();
|
|
pwmMax[i]->save();
|
|
pwmDeadZone[i]->save();
|
|
filter[i]->save();
|
|
reverse[i]->save();
|
|
}
|
|
|
|
// load
|
|
if (loadEEProm)
|
|
{
|
|
scale[i]->load();
|
|
pwmMin[i]->load();
|
|
pwmNeutral[i]->load();
|
|
pwmMax[i]->load();
|
|
pwmDeadZone[i]->load();
|
|
filter[i]->load();
|
|
reverse[i]->load();
|
|
}
|
|
|
|
// find neutral position
|
|
AP_RcChannel * ch = new AP_RcChannel(APM_RC,i,scale[i]->get(),
|
|
pwmMin[i]->get(),pwmNeutral[i]->get(),pwmMax[i]->get(),
|
|
pwmDeadZone[i]->get(),filter[i]->get(),reverse[i]->get());
|
|
|
|
ch->readRadio();
|
|
pwmNeutral[i]->set(ch->getPwm());
|
|
|
|
// add rc channel
|
|
rc.push_back(ch);
|
|
}
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
// read radio
|
|
for (int i=0;i<nChannels;i++) rc[i]->readRadio();
|
|
|
|
// read test positions
|
|
Serial.printf("\npwm :\t");
|
|
for (int i=0;i<nChannels;i++) Serial.printf("%7d\t",rc[i]->getPwm());
|
|
Serial.println();
|
|
Serial.printf("position :\t");
|
|
for (int i=0;i<nChannels;i++) Serial.printf("%7.2f\t",rc[i]->getPosition());
|
|
Serial.println();
|
|
delay(100);
|
|
}
|