Browse Source

Copter: disable aux channel 7 for SingleCopter

Move servo range setup to AP_MotorsSingle
master
Randy Mackay 11 years ago
parent
commit
7c25247c87
  1. 20
      libraries/AP_Motors/AP_MotorsSingle.cpp
  2. 2
      libraries/AP_Motors/AP_MotorsSingle.h

20
libraries/AP_Motors/AP_MotorsSingle.cpp

@ -95,11 +95,21 @@ void AP_MotorsSingle::Init() @@ -95,11 +95,21 @@ void AP_MotorsSingle::Init()
// set update rate for the 3 motors (but not the servo on channel 7)
set_update_rate(_speed_hz);
// set the motor_enabled flag so that the ESCs can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_1] = true;
motor_enabled[AP_MOTORS_MOT_2] = true;
motor_enabled[AP_MOTORS_MOT_3] = true;
motor_enabled[AP_MOTORS_MOT_4] = true;
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_7] = true;
// we set four servos to angle
_servo1->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo2->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo3->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo4->set_type(RC_CHANNEL_TYPE_ANGLE);
_servo1->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo2->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo3->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
_servo4->set_angle(AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
// disable CH7 from being used as an aux output (i.e. for camera gimbal, etc)
RC_Channel_aux::disable_aux_channel(CH_7);
}
// set update rate to motors - a value in hertz

2
libraries/AP_Motors/AP_MotorsSingle.h

@ -18,6 +18,8 @@ @@ -18,6 +18,8 @@
#define AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS 250 // update rate for digital servos
#define AP_MOTORS_SINGLE_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
#define AP_MOTORS_SINGLE_SERVO_INPUT_RANGE 4500 // roll or pitch input of -4500 will cause servos to their minimum (i.e. radio_min), +4500 will move them to their maximum (i.e. radio_max)
/// @class AP_MotorsSingle
class AP_MotorsSingle : public AP_Motors {
public:

Loading…
Cancel
Save