// init
void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
add_motor_num(AP_MOTORS_MOT_1);
add_motor_num(AP_MOTORS_MOT_2);
add_motor_num(AP_MOTORS_MOT_4);
// 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_4] = true;
// find the yaw servo
_yaw_servo = SRV_Channels::get_channel_for(SRV_Channel::k_motor7, AP_MOTORS_CH_TRI_YAW);
if (!_yaw_servo) {
gcs().send_text(MAV_SEVERITY_ERROR, "MotorsTri: unable to setup yaw channel");
// don't set initialised_ok
return;
}
// allow mapping of motor7
add_motor_num(AP_MOTORS_CH_TRI_YAW);
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_TRI);
}
// init
void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// 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 main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = 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);
// allow mapping of motor7
add_motor_num(CH_7);
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);
}
// init
void AP_MotorsTri::Init()
{
add_motor_num(AP_MOTORS_MOT_1);
add_motor_num(AP_MOTORS_MOT_2);
add_motor_num(AP_MOTORS_MOT_4);
// 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_4] = true;
// allow mapping of motor7
add_motor_num(AP_MOTORS_CH_TRI_YAW);
}
// init
void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
add_motor_num(AP_MOTORS_MOT_1);
add_motor_num(AP_MOTORS_MOT_2);
add_motor_num(AP_MOTORS_MOT_4);
// 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_4] = true;
// allow mapping of motor7
add_motor_num(AP_MOTORS_CH_TRI_YAW);
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_TRI);
}
// add motor using separate throttle, steering and lateral factors for frames with custom motor configurations
void AP_MotorsUGV::add_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// set throttle, steering and lateral factors
_throttle_factor[motor_num] = throttle_factor;
_steering_factor[motor_num] = steering_factor;
_lateral_factor[motor_num] = lateral_factor;
add_motor_num(motor_num);
}
}
// init_outputs - initialise Servo/PWM ranges and endpoints
bool AP_MotorsHeli_Single::init_outputs()
{
if (!_flags.initialised_ok) {
// map primary swash servos
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
// yaw servo
add_motor_num(CH_4);
// initialize main rotor servo
_main_rotor.init_servo();
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.init_servo();
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// external gyro output
add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO);
}
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// External Gyro uses PWM output thus servo endpoints are forced
SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000);
}
// reset swash servo range and endpoints
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
// yaw servo is an angle from -4500 to 4500
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
_flags.initialised_ok = true;
return true;
}
// init
void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// make sure 6 output channels are mapped
for (uint8_t i=0; i<6; i++) {
add_motor_num(CH_1+i);
}
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = true;
// setup actuator scaling
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
// record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);
}
// init_outputs
bool AP_MotorsHeli_Dual::init_outputs()
{
if (!_flags.initialised_ok) {
// make sure 6 output channels are mapped
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
// set rotor servo range
_rotor.init_servo();
}
// reset swash servo range and endpoints
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
_flags.initialised_ok = true;
return true;
}
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
{
// ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// increment number of motors if this motor is being newly motor_enabled
if( !motor_enabled[motor_num] ) {
motor_enabled[motor_num] = true;
}
// set roll, pitch, thottle factors and opposite motor (for stability patch)
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
// set order that motor appears in test
_test_order[motor_num] = testing_order;
// call parent class method
add_motor_num(motor_num);
}
}
// init
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 main ESC can be calibrated like other frame types
motor_enabled[AP_MOTORS_MOT_5] = true;
motor_enabled[AP_MOTORS_MOT_6] = 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);
// allow mapping of motor7
add_motor_num(CH_7);
}
