AP_Motors: Heli: remove _output_test_seq as used in motor test

This commit is contained in:
Iampete1 2023-06-18 19:52:18 +01:00 committed by Randy Mackay
parent 753c56f97a
commit c5733e7634
7 changed files with 3 additions and 112 deletions

View File

@ -159,6 +159,9 @@ public:
// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const override;
// output_test_seq - disabled on heli, do nothing
void _output_test_seq(uint8_t motor_seq, int16_t pwm) override {};
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

View File

@ -265,47 +265,6 @@ bool AP_MotorsHeli_Dual::init_outputs()
return true;
}
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Dual::_output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// swash servo 2
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// swash servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// swash servo 4
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// swash servo 5
rc_write(AP_MOTORS_MOT_5, pwm);
break;
case 6:
// swash servo 6
rc_write(AP_MOTORS_MOT_6, pwm);
break;
case 7:
// main rotor
rc_write(AP_MOTORS_HELI_RSC, pwm);
break;
default:
// do nothing
break;
}
}
// calculate_armed_scalars
void AP_MotorsHeli_Dual::calculate_armed_scalars()
{

View File

@ -46,9 +46,6 @@ public:
// set_update_rate - set update rate to motors
void set_update_rate( uint16_t speed_hz ) override;
// output_test_seq - spin a motor at the pwm value specified
virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
// output_to_motors - sends values out to the motors
void output_to_motors() override;

View File

@ -65,26 +65,6 @@ bool AP_MotorsHeli_Quad::init_outputs()
return true;
}
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Quad::_output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// output to motors and servos
switch (motor_seq) {
case 1 ... AP_MOTORS_HELI_QUAD_NUM_MOTORS:
rc_write(AP_MOTORS_MOT_1 + (motor_seq-1), pwm);
break;
case AP_MOTORS_HELI_QUAD_NUM_MOTORS+1:
// main rotor
rc_write(AP_MOTORS_HELI_RSC, pwm);
break;
default:
// do nothing
break;
}
}
// calculate_armed_scalars
void AP_MotorsHeli_Quad::calculate_armed_scalars()
{

View File

@ -63,9 +63,6 @@ protected:
// move_actuators - moves swash plate to attitude of parameters passed in
void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override;
// output_test_seq - spin a motor at the pwm value specified
virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
const char* _get_frame_string() const override { return "HELI_QUAD"; }
// rate factors

View File

@ -248,46 +248,6 @@ bool AP_MotorsHeli_Single::init_outputs()
return true;
}
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Single::_output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// swash servo 2
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// swash servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// external gyro & tail servo
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
if (_acro_tail && _ext_gyro_gain_acro > 0) {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, _ext_gyro_gain_acro);
} else {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, _ext_gyro_gain_std);
}
}
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// main rotor
rc_write(AP_MOTORS_HELI_RSC, pwm);
break;
default:
// do nothing
break;
}
}
// set_desired_rotor_speed
void AP_MotorsHeli_Single::set_desired_rotor_speed(float desired_speed)
{

View File

@ -106,11 +106,6 @@ protected:
// servo_test - move servos through full range of movement
void servo_test() override;
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
// external objects we depend upon
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
AP_MotorsHeli_Swash _swashplate; // swashplate