Sub: Add forward and strafe input channels

This commit is contained in:
Jacob Walser 2017-02-07 22:51:37 -05:00 committed by Andrew Tridgell
parent 2bacc2fea2
commit 9eda7f1b5b
2 changed files with 8 additions and 0 deletions

View File

@ -162,6 +162,8 @@ private:
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
RC_Channel *channel_forward;
RC_Channel *channel_strafe;
// Dataflash
DataFlash_Class DataFlash{FIRMWARE_STRING};

View File

@ -27,16 +27,22 @@ void Copter::init_rc_in()
channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
channel_forward = RC_Channel::rc_channel(rcmap.forward()-1);
channel_strafe = RC_Channel::rc_channel(rcmap.strafe()-1);
// set rc channel ranges
channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX);
channel_yaw->set_angle(4500);
channel_throttle->set_range(g.throttle_min, THR_MAX);
channel_forward->set_angle(4500);
channel_strafe->set_angle(4500);
channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_forward->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_strafe->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
//set auxiliary servo ranges
g.rc_5.set_range(0,1000);