Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Heli: Add blade pitch angle logging #26606

Merged
merged 1 commit into from
May 5, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -612,3 +612,14 @@ void AP_MotorsHeli::set_rotor_runup_complete(bool new_value)
#endif
_heliflags.rotor_runup_complete = new_value;
}

#if HAL_LOGGING_ENABLED
// Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion
float AP_MotorsHeli::get_cyclic_angle_scaler(void) const {
// We want to use the collective min-max to angle relationship to calculate the cyclic input to angle relationship
// First we scale the collective angle range by it's min-max output. Recall that we assume that the maximum possible
// collective range is 1000, hence the *1e-3.
// The factor 2.0 accounts for the fact that we scale the servo outputs from 0 ~ 1 to -1 ~ 1
return ((float)(_collective_max-_collective_min))*1e-3 * (_collective_max_deg.get() - _collective_min_deg.get()) * 2.0;
}
#endif
5 changes: 5 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli.h
Original file line number Diff line number Diff line change
Expand Up @@ -236,6 +236,11 @@ class AP_MotorsHeli : public AP_Motors {
// Update _heliflags.rotor_runup_complete value writing log event on state change
void set_rotor_runup_complete(bool new_value);

#if HAL_LOGGING_ENABLED
// Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion for blade angle logging
float get_cyclic_angle_scaler(void) const;
#endif

// enum values for HOVER_LEARN parameter
enum HoverLearn {
HOVER_LEARN_DISABLED = 0,
Expand Down
9 changes: 9 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -594,3 +594,12 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const

return true;
}

#if HAL_LOGGING_ENABLED
// Blade angle logging - called at 10 Hz
void AP_MotorsHeli_Dual::Log_Write(void)
{
_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());
}
#endif
9 changes: 7 additions & 2 deletions libraries/AP_Motors/AP_MotorsHeli_Dual.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,11 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli {
// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const override;

#if HAL_LOGGING_ENABLED
// Blade angle 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[];

Expand All @@ -76,8 +81,8 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli {
const char* _get_frame_string() const override { return "HELI_DUAL"; }

// objects we depend upon
AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7 }; // swashplate1
AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8 }; // swashplate2
AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7, 1U }; // swashplate1
AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8, 2U }; // swashplate2

// internal variables
float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function
Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli_Single.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -691,3 +691,14 @@ bool AP_MotorsHeli_Single::use_tail_RSC() const
return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) ||
(type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV);
}

#if HAL_LOGGING_ENABLED
void AP_MotorsHeli_Single::Log_Write(void)
{
// 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());
}
#endif
7 changes: 6 additions & 1 deletion libraries/AP_Motors/AP_MotorsHeli_Single.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
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),
_swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5)
_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);
};
Expand Down Expand Up @@ -77,6 +77,11 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
// Thrust Linearization handling
Thrust_Linearization thr_lin {*this};

#if HAL_LOGGING_ENABLED
// Blade angle logging - called at 10 Hz
void Log_Write(void) override;
#endif

// var_info
static const struct AP_Param::GroupInfo var_info[];

Expand Down
42 changes: 41 additions & 1 deletion libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
AP_GROUPEND
};

AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3)
AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t instance) :
_instance(instance)
{
_motor_num[0] = mot_0;
_motor_num[1] = mot_1;
Expand Down Expand Up @@ -219,6 +220,11 @@ void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective)
collective = 1 - collective;
}

// Store inputs for logging
_roll_input = roll;
_pitch_input = pitch;
_collective_input_scaled = collective;

for (uint8_t i = 0; i < _max_num_servos; i++) {
if (!_enabled[i]) {
// This servo is not enabled
Expand Down Expand Up @@ -283,3 +289,37 @@ uint32_t AP_MotorsHeli_Swash::get_output_mask() const
}
return mask;
}

#if HAL_LOGGING_ENABLED
// Write SWSH log for this instance of swashplate
void AP_MotorsHeli_Swash::write_log(float cyclic_scaler, float col_ang_min, float col_ang_max, int16_t col_min, int16_t col_max) const
{
// Calculate the collective contribution to blade pitch angle
// Swashplate receives the scaled collective value based on the col_min and col_max params. We have to reverse the scaling here to do the angle calculation.
float collective_scalar = ((float)(col_max-col_min))*1e-3;
collective_scalar = MAX(collective_scalar, 1e-3);
float _collective_input = (_collective_input_scaled - (float)(col_min - 1000)*1e-3) / collective_scalar;
float col = (col_ang_max - col_ang_min) * _collective_input + col_ang_min;

// Calculate the cyclic contribution to blade pitch angle
float tcyc = norm(_roll_input, _pitch_input) * cyclic_scaler;
float pcyc = _pitch_input * cyclic_scaler;
float rcyc = _roll_input * cyclic_scaler;

// @LoggerMessage: SWSH
// @Description: Helicopter swashplate logging
// @Field: TimeUS: Time since system startup
// @Field: I: Swashplate instance
// @Field: Col: Blade pitch angle contribution from collective
// @Field: TCyc: Total blade pitch angle contribution from cyclic
// @Field: PCyc: Blade pitch angle contribution from pitch cyclic
// @Field: RCyc: Blade pitch angle contribution from roll cyclic
AP::logger().WriteStreaming("SWSH", "TimeUS,I,Col,TCyc,PCyc,RCyc", "s#dddd", "F-0000", "QBffff",
AP_HAL::micros64(),
_instance,
col,
tcyc,
pcyc,
rcyc);
}
#endif
14 changes: 13 additions & 1 deletion libraries/AP_Motors/AP_MotorsHeli_Swash.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Param/AP_Param.h>
#include <AP_Logger/AP_Logger.h>

// swashplate types
enum SwashPlateType {
Expand All @@ -19,7 +20,7 @@ enum SwashPlateType {
class AP_MotorsHeli_Swash {
public:

AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3);
AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t instance);

// configure - configure the swashplate settings for any updated parameters
void configure();
Expand All @@ -39,6 +40,11 @@ class AP_MotorsHeli_Swash {
// Get function output mask
uint32_t get_output_mask() const;

#if HAL_LOGGING_ENABLED
MattKear marked this conversation as resolved.
Show resolved Hide resolved
// Write SWSH log for this instance of swashplate
void write_log(float cyclic_scaler, float col_ang_min, float col_ang_max, int16_t col_min, int16_t col_max) const;
#endif

// var_info
static const struct AP_Param::GroupInfo var_info[];

Expand Down Expand Up @@ -76,6 +82,12 @@ class AP_MotorsHeli_Swash {
float _collectiveFactor[_max_num_servos]; // Collective axis scaling of servo output based on servo position
float _output[_max_num_servos]; // Servo output value
uint8_t _motor_num[_max_num_servos]; // Motor function to use for output
const uint8_t _instance; // Swashplate instance. Used for logging.

// Variables stored for logging
float _roll_input;
float _pitch_input;
float _collective_input_scaled;

// parameters
AP_Int8 _swashplate_type; // Swash Type Setting
Expand Down
Loading