From 7271a469fd3de2adc01a112762c83ecd1cf58899 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Aug 2024 15:59:17 +0100 Subject: [PATCH] AP_MotorsHeli: Move heli logging down to motors --- libraries/AP_Motors/AP_MotorsHeli.h | 11 +----- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 6 +++- libraries/AP_Motors/AP_MotorsHeli_Dual.h | 2 +- libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 8 +++++ libraries/AP_Motors/AP_MotorsHeli_Quad.h | 5 +++ libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 37 +++++++++++++++----- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 19 ++++++---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 7 +++- libraries/AP_Motors/AP_MotorsHeli_Single.h | 2 +- 9 files changed, 68 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 3aa44d2b43..bc28116a9e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -42,7 +42,7 @@ public: /// Constructor AP_MotorsHeli( uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : AP_Motors(speed_hz), - _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC) + _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC, 0U) { AP_Param::setup_object_defaults(this, var_info); }; @@ -90,18 +90,9 @@ public: // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1 float get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); } - // get_main_rotor_speed - estimated rotor speed when no governor or speed sensor used - float get_main_rotor_speed() const { return _main_rotor.get_rotor_speed(); } - // return true if the main rotor is up to speed bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; } - //get rotor governor output - float get_governor_output() const { return _main_rotor.get_governor_output(); } - - //get engine throttle output - float get_control_output() const { return _main_rotor.get_control_output(); } - // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint32_t get_motor_mask() override; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index f2cc35f224..81ac8846aa 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -597,10 +597,14 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const } #if HAL_LOGGING_ENABLED -// Blade angle logging - called at 10 Hz +// heli motors logging - called at 10 Hz void AP_MotorsHeli_Dual::Log_Write(void) { + // write swashplate log _swashplate1.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get()); _swashplate2.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective2_min.get(), _collective2_max.get()); + + // write RSC log + _main_rotor.write_log(); } #endif diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 2fb696eb91..057a4983e4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -57,7 +57,7 @@ public: bool arming_checks(size_t buflen, char *buffer) const override; #if HAL_LOGGING_ENABLED - // Blade angle logging - called at 10 Hz + // heli motors logging - called at 10 Hz void Log_Write(void) override; #endif diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 817e89f1ac..7ced336992 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -279,3 +279,11 @@ void AP_MotorsHeli_Quad::servo_test() { // not implemented } + +#if HAL_LOGGING_ENABLED +// heli motors logging - called at 10 Hz +void AP_MotorsHeli_Quad::Log_Write(void) +{ + _main_rotor.write_log(); +} +#endif diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.h b/libraries/AP_Motors/AP_MotorsHeli_Quad.h index a67cb40911..095d1b19f6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.h @@ -40,6 +40,11 @@ public: // servo_test - move servos through full range of movement void servo_test() override; +#if HAL_LOGGING_ENABLED + // heli motors logging - called at 10 Hz + void Log_Write(void) override; +#endif + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index a54c583d6a..960acbb7fe 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -480,20 +480,13 @@ void AP_MotorsHeli_RSC::update_rotor_runup(float dt) _runup_complete = false; } // if rotor estimated speed is zero, then spooldown has been completed - if (get_rotor_speed() <= 0.0f) { + if (_rotor_runup_output <= 0.0f) { _spooldown_complete = true; } else { _spooldown_complete = false; } } -// get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value -float AP_MotorsHeli_RSC::get_rotor_speed() const -{ - // if no actual measured rotor speed is available, estimate speed based on rotor runup scalar. - return _rotor_runup_output; -} - // write_rsc - outputs pwm onto output rsc channel // servo_out parameter is of the range 0 ~ 1 void AP_MotorsHeli_RSC::write_rsc(float servo_out) @@ -616,3 +609,31 @@ void AP_MotorsHeli_RSC::governor_reset() _governor_engage = false; _governor_fault_count = 0; // reset fault count when governor reset } + +#if HAL_LOGGING_ENABLED +// Write a helicopter motors packet +void AP_MotorsHeli_RSC::write_log(void) const +{ + // @LoggerMessage: HRSC + // @Description: Helicopter related messages + // @Field: I: Instance, 0=Main, 1=Tail + // @Field: TimeUS: Time since system startup + // @Field: DRRPM: Desired rotor speed + // @Field: ERRPM: Estimated rotor speed + // @Field: Gov: Governor Output + // @Field: Throt: Throttle output + + // Write to data flash log + AP::logger().WriteStreaming("HRSC", + "TimeUS,I,DRRPM,ERRPM,Gov,Throt", + "s#----", + "F-----", + "QBffff", + AP_HAL::micros64(), + _instance, + get_desired_speed(), + _rotor_runup_output, + _governor_output, + get_control_output()); +} +#endif diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 03b3500c9b..e4b5ba4951 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -4,6 +4,7 @@ #include // ArduPilot Mega Vector/Matrix math Library #include #include +#include // default main rotor speed (ch8 out) as a number from 0 ~ 100 #define AP_MOTORS_HELI_RSC_SETPOINT 70 @@ -46,9 +47,11 @@ public: friend class AP_MotorsHeli_Quad; AP_MotorsHeli_RSC(SRV_Channel::Aux_servo_function_t aux_fn, - uint8_t default_channel) : + uint8_t default_channel, + uint8_t inst) : _aux_fn(aux_fn), - _default_channel(default_channel) + _default_channel(default_channel), + _instance(inst) { AP_Param::setup_object_defaults(this, var_info); }; @@ -81,12 +84,8 @@ public: // set_desired_speed - this requires input to be 0-1 void set_desired_speed(float desired_speed) { _desired_speed = desired_speed; } - // get_rotor_speed - estimated rotor speed when no governor or rpm sensor is used - float get_rotor_speed() const; - // functions for autothrottle, throttle curve, governor, idle speed, output to servo void set_governor_output(float governor_output) {_governor_output = governor_output; } - float get_governor_output() const { return _governor_output; } void governor_reset(); float get_control_output() const { return _control_output; } void set_idle_output(float idle_output) { _idle_output.set(idle_output); } @@ -129,7 +128,12 @@ public: uint32_t get_output_mask() const; // rotor_speed_above_critical - return true if rotor speed is above that critical for flight - bool rotor_speed_above_critical(void) const { return get_rotor_speed() >= get_critical_speed(); } + bool rotor_speed_above_critical(void) const { return _rotor_runup_output >= get_critical_speed(); } + +#if HAL_LOGGING_ENABLED + // RSC logging + void write_log(void) const; +#endif // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -147,6 +151,7 @@ public: private: uint64_t _last_update_us; + const uint8_t _instance; // channel setup for aux function SRV_Channel::Aux_servo_function_t _aux_fn; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 33cc445904..ad0e7fff83 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -467,7 +467,7 @@ float AP_MotorsHeli_Single::get_yaw_offset(float collective) return 0.0; } - if (_heliflags.in_autorotation || (get_control_output() <= _main_rotor.get_idle_output())) { + if (_heliflags.in_autorotation || (_main_rotor.get_control_output() <= _main_rotor.get_idle_output())) { // Motor is stopped or at idle, and thus not creating torque return 0.0; } @@ -695,10 +695,15 @@ bool AP_MotorsHeli_Single::use_tail_RSC() const #if HAL_LOGGING_ENABLED void AP_MotorsHeli_Single::Log_Write(void) { + // Write swash plate logging // For single heli we have to apply an additional cyclic scaler of sqrt(2.0) because the // definition of when we achieve _cyclic_max is different to dual heli. In single, _cyclic_max // is limited at sqrt(2.0), in dual it is limited at 1.0 float cyclic_angle_scaler = get_cyclic_angle_scaler() * sqrtf(2.0); _swashplate.write_log(cyclic_angle_scaler, _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get()); + + // Write RSC logging + _main_rotor.write_log(); + _tail_rotor.write_log(); } #endif diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 72754a1134..5b55ffad09 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -32,7 +32,7 @@ public: // constructor AP_MotorsHeli_Single(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : AP_MotorsHeli(speed_hz), - _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC), + _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC, 1U), _swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5, 1U) { AP_Param::setup_object_defaults(this, var_info);