Tracker: add read_radio

This commit is contained in:
Randy Mackay 2014-10-06 17:01:54 +09:00
parent f4d45c9b6d
commit 1b7ca684ee
3 changed files with 25 additions and 2 deletions

View File

@ -182,8 +182,8 @@ SITL sitl;
/**
antenna control channels
*/
static RC_Channel channel_yaw(CH_1);
static RC_Channel channel_pitch(CH_2);
static RC_Channel channel_yaw(CH_YAW);
static RC_Channel channel_pitch(CH_PITCH);
////////////////////////////////////////////////////////////////////////////////
// GCS selection
@ -255,6 +255,7 @@ static struct {
*/
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ update_ahrs, 1, 1000 },
{ read_radio, 1, 200 },
{ update_tracking, 1, 1000 },
{ update_GPS, 5, 4000 },
{ update_compass, 5, 1500 },

View File

@ -55,6 +55,17 @@
# define SERIAL2_BUFSIZE 256
#endif
//////////////////////////////////////////////////////////////////////////////
// RC Channel definitions
//
#ifndef CH_YAW
# define CH_YAW CH_1 // RC input/output for yaw on channel 1
#endif
#ifndef CH_PITCH
# define CH_PITCH CH_2 // RC input/output for pitch on channel 2
#endif
//////////////////////////////////////////////////////////////////////////////
// yaw and pitch axis angle range defaults
//

11
AntennaTracker/radio.pde Normal file
View File

@ -0,0 +1,11 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Functions to read the RC radio input
static void read_radio()
{
if (hal.rcin->new_input()) {
channel_yaw.set_pwm(hal.rcin->read(CH_YAW));
channel_pitch.set_pwm(hal.rcin->read(CH_PITCH));
}
}