diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index e2c0971fdc..f599c4e8eb 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -27,32 +27,9 @@ extern const AP_HAL::HAL& hal; -const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = { - // variables from parent vehicle - AP_NESTEDGROUPINFO(AP_MotorsMulticopter, 0), - - // parameters 1 ~ 29 were reserved for tradheli - // parameters 30 ~ 39 reserved for tricopter - // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) - - // 40 was ROLL_SV_REV - // 41 was PITCH_SV_REV - // 42 was YAW_SV_REV - - // @Param: SV_SPEED - // @DisplayName: Servo speed - // @Description: Servo update speed - // @Units: Hz - AP_GROUPINFO("SV_SPEED", 43, AP_MotorsCoax, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS), - - AP_GROUPEND -}; // init void AP_MotorsCoax::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); - _servo1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1); _servo2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2); _servo3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3); @@ -89,17 +66,10 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) // record requested speed _speed_hz = speed_hz; - // set update rate for the 4 servos and 2 motors uint32_t mask = - 1U << AP_MOTORS_MOT_1 | - 1U << AP_MOTORS_MOT_2 | - 1U << AP_MOTORS_MOT_3 | - 1U << AP_MOTORS_MOT_4 ; - rc_set_freq(mask, _servo_speed); - uint32_t mask2 = 1U << AP_MOTORS_MOT_5 | 1U << AP_MOTORS_MOT_6 ; - rc_set_freq(mask2, _speed_hz); + rc_set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 92a9de82aa..98f7ddc1df 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -26,7 +26,6 @@ public: AP_MotorsCoax(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMulticopter(loop_rate, speed_hz) { - AP_Param::setup_object_defaults(this, var_info); }; // init @@ -53,16 +52,10 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask(); - // var_info for holding Parameter information - static const struct AP_Param::GroupInfo var_info[]; - protected: // output - sends commands to the motors void output_armed_stabilizing(); - // servo speed - AP_Int16 _servo_speed; - float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_yt_ccw; float _thrust_yt_cw; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 8a3dd43dc3..004918f1bc 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -145,6 +145,15 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("SAFE_DISARM", 23, AP_MotorsMulticopter, _disarm_disable_pwm, 0), + // @Param: YAW_SV_ANGLE + // @DisplayName: Yaw Servo Max Lean Angle + // @Description: Yaw servo's maximum lean angle + // @Range: 5 80 + // @Units: Degrees + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("YAW_SV_ANGLE", 35, AP_MotorsMulticopter, _yaw_servo_angle_max_deg, 30), + AP_GROUPEND }; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index c3996ffab7..01e438c02f 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -163,6 +163,9 @@ protected: AP_Int8 _throttle_hover_learn; // enable/disabled hover thrust learning AP_Int8 _disarm_disable_pwm; // disable PWM output while disarmed + // Maximum lean angle of yaw servo in degrees. This is specific to tricopter + AP_Float _yaw_servo_angle_max_deg; + // motor output variables bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled int16_t _throttle_radio_min; // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN) diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index dd4c57c951..8bc103cd8e 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -27,26 +27,6 @@ extern const AP_HAL::HAL& hal; -const AP_Param::GroupInfo AP_MotorsSingle::var_info[] = { - // variables from parent vehicle - AP_NESTEDGROUPINFO(AP_MotorsMulticopter, 0), - - // parameters 1 ~ 29 were reserved for tradheli - // parameters 30 ~ 39 reserved for tricopter - // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) - - // 40 was ROLL_SV_REV - // 41 was PITCH_SV_REV - // 42 was YAW_SV_REV - - // @Param: SV_SPEED - // @DisplayName: Servo speed - // @Description: Servo update speed in hz - // @Values: 50, 125, 250 - AP_GROUPINFO("SV_SPEED", 43, AP_MotorsSingle, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS), - - AP_GROUPEND -}; // init void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type) { @@ -92,17 +72,10 @@ void AP_MotorsSingle::set_update_rate( uint16_t speed_hz ) // record requested speed _speed_hz = speed_hz; - // set update rate for the 3 motors (but not the servo on channel 7) - uint32_t mask = - 1U << AP_MOTORS_MOT_1 | - 1U << AP_MOTORS_MOT_2 | - 1U << AP_MOTORS_MOT_3 | - 1U << AP_MOTORS_MOT_4 ; - rc_set_freq(mask, _servo_speed); - uint32_t mask2 = + uint32_t mask = 1U << AP_MOTORS_MOT_5 | 1U << AP_MOTORS_MOT_6 ; - rc_set_freq(mask2, _speed_hz); + rc_set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index 8aa059ee38..d0626ea016 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -26,7 +26,6 @@ public: AP_MotorsSingle(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMulticopter(loop_rate, speed_hz) { - AP_Param::setup_object_defaults(this, var_info); }; // init @@ -53,16 +52,10 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask(); - // var_info for holding Parameter information - static const struct AP_Param::GroupInfo var_info[]; - protected: // output - sends commands to the motors void output_armed_stabilizing(); - // servo speed - AP_Int16 _servo_speed; - int16_t _throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_out; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index d577b5844d..8633bc1117 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -33,15 +33,6 @@ const AP_Param::GroupInfo AP_MotorsTri::var_info[] = { // parameters 30 ~ 39 reserved for tricopter // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) - // @Param: YAW_SV_ANGLE - // @DisplayName: Yaw Servo Max Lean Angle - // @Description: Yaw servo's maximum lean angle - // @Range: 5 80 - // @Units: Degrees - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("YAW_SV_ANGLE", 35, AP_MotorsTri, _yaw_servo_angle_max_deg, 30), - AP_GROUPEND }; diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 75d7ff1ecd..87d8c12f50 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -64,7 +64,6 @@ protected: // parameters SRV_Channel *_yaw_servo; // yaw output channel - AP_Float _yaw_servo_angle_max_deg; // Maximum lean angle of yaw servo in degrees float _pivot_angle; // Angle of yaw pivot float _thrust_right; float _thrust_rear;