From e4ce23b05e0940e08ff6fbfa874f93086e8ff526 Mon Sep 17 00:00:00 2001 From: Jordan L Date: Thu, 16 Nov 2023 14:46:11 -0500 Subject: [PATCH] AP_MotorsPulsing_Heli revert to include rpm the rpm parameter was erroniously deleted, and needs to be re-added --- libraries/AP_Motors/AP_MotorsPulsing_Heli.cpp | 2 ++ libraries/AP_Motors/AP_MotorsPulsing_Heli.h | 1 + 2 files changed, 3 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsPulsing_Heli.cpp b/libraries/AP_Motors/AP_MotorsPulsing_Heli.cpp index a65225c220187..e49494f6d6b20 100644 --- a/libraries/AP_Motors/AP_MotorsPulsing_Heli.cpp +++ b/libraries/AP_Motors/AP_MotorsPulsing_Heli.cpp @@ -57,6 +57,7 @@ void AP_MotorsPulsing_Heli::init(motor_frame_class frame_class, motor_frame_type motor_enabled[AP_MOTORS_MOT_4] = true; _mav_type = MAV_TYPE_QUADROTOR; + rpm = AP_RPM::get_singleton(); // record successful initialisation if what we setup was the desired frame_class set_initialised_ok(frame_class == MOTOR_FRAME_PULSING_HELI); @@ -148,6 +149,7 @@ void AP_MotorsPulsing_Heli::output_armed_stabilizing() // gyro ff and yaw ff. Also need rotor height above COM Vector3f gyro_latest = _ahrs_view->get_gyro_latest(); float velocity_0 = 0; + rpm->get_rpm(0, velocity_0); float rotor_yaw_ff = _rotor_yaw_ff * velocity_0 * velocity_0; Vector3f blade_omega(gyro_latest.x, gyro_latest.y, gyro_latest.z-velocity_0); diff --git a/libraries/AP_Motors/AP_MotorsPulsing_Heli.h b/libraries/AP_Motors/AP_MotorsPulsing_Heli.h index 60b74c6a531f7..df566f1bb4e1c 100644 --- a/libraries/AP_Motors/AP_MotorsPulsing_Heli.h +++ b/libraries/AP_Motors/AP_MotorsPulsing_Heli.h @@ -56,6 +56,7 @@ class AP_MotorsPulsing_Heli : public AP_MotorsMulticopter { float _roll_action; AP_AHRS_View *_ahrs_view; + AP_RPM *rpm; AP_Int8 _yaw_dir; AP_Float _rotor_yaw_ff;