Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Heli: move heli parameter check to arming check

This commit is contained in:
Iampete1 2023-07-18 21:16:48 +01:00 committed by Randy Mackay
parent 0aef2cc133
commit c5f3d5a98b
6 changed files with 72 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -440,69 +440,6 @@ void AP_MotorsHeli::output_logic()
}
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli::parameter_check(bool display_msg) const
{
// returns false if RSC Mode is not set to a valid control mode
if (_main_rotor._rsc_mode.get() <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _main_rotor._rsc_mode.get() > (int8_t)ROTOR_CONTROL_MODE_AUTOTHROTTLE) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_MODE invalid");
}
return false;
}
// returns false if rsc_setpoint is out of range
if ( _main_rotor._rsc_setpoint.get() > 100 || _main_rotor._rsc_setpoint.get() < 10){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_SETPOINT out of range");
}
return false;
}
// returns false if idle output is out of range
if ( _main_rotor._idle_output.get() > 100 || _main_rotor._idle_output.get() < 0){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_IDLE out of range");
}
return false;
}
// returns false if _rsc_critical is not between 0 and 100
if (_main_rotor._critical_speed.get() > 100 || _main_rotor._critical_speed.get() < 0) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_CRITICAL out of range");
}
return false;
}
// returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate
if (_main_rotor._runup_time.get() <= _main_rotor._ramp_time.get()){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RUNUP_TIME too small");
}
return false;
}
// returns false if _collective_min_deg is not default value which indicates users set parameter
if (is_equal((float)_collective_min_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MIN_DEG)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_ANG_MIN to measured min blade pitch in deg");
}
return false;
}
// returns false if _collective_max_deg is not default value which indicates users set parameter
if (is_equal((float)_collective_max_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MAX_DEG)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_ANG_MAX to measured max blade pitch in deg");
}
return false;
}
// all other cases parameters are OK
return true;
}
// update the throttle input filter
void AP_MotorsHeli::update_throttle_filter()
{
@ -576,6 +513,7 @@ void AP_MotorsHeli::update_turbine_start()
}
}
// Run arming checks
bool AP_MotorsHeli::arming_checks(size_t buflen, char *buffer) const
{
// run base class checks
@ -588,6 +526,48 @@ bool AP_MotorsHeli::arming_checks(size_t buflen, char *buffer) const
return false;
}
// returns false if RSC Mode is not set to a valid control mode
if (_main_rotor._rsc_mode.get() <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _main_rotor._rsc_mode.get() > (int8_t)ROTOR_CONTROL_MODE_AUTOTHROTTLE) {
hal.util->snprintf(buffer, buflen, "H_RSC_MODE invalid");
return false;
}
// returns false if rsc_setpoint is out of range
if ( _main_rotor._rsc_setpoint.get() > 100 || _main_rotor._rsc_setpoint.get() < 10){
hal.util->snprintf(buffer, buflen, "H_RSC_SETPOINT out of range");
return false;
}
// returns false if idle output is out of range
if ( _main_rotor._idle_output.get() > 100 || _main_rotor._idle_output.get() < 0){
hal.util->snprintf(buffer, buflen, "H_RSC_IDLE out of range");
return false;
}
// returns false if _rsc_critical is not between 0 and 100
if (_main_rotor._critical_speed.get() > 100 || _main_rotor._critical_speed.get() < 0) {
hal.util->snprintf(buffer, buflen, "H_RSC_CRITICAL out of range");
return false;
}
// returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate
if (_main_rotor._runup_time.get() <= _main_rotor._ramp_time.get()){
hal.util->snprintf(buffer, buflen, "H_RUNUP_TIME too small");
return false;
}
// returns false if _collective_min_deg is not default value which indicates users set parameter
if (is_equal((float)_collective_min_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MIN_DEG)) {
hal.util->snprintf(buffer, buflen, "Set H_COL_ANG_MIN to measured min blade pitch in deg");
return false;
}
// returns false if _collective_max_deg is not default value which indicates users set parameter
if (is_equal((float)_collective_max_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MAX_DEG)) {
hal.util->snprintf(buffer, buflen, "Set H_COL_ANG_MAX to measured max blade pitch in deg");
return false;
}
return true;
}

3
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -66,9 +66,6 @@ public:
// heli specific methods
//
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
virtual bool parameter_check(bool display_msg) const;
//set turbine start flag on to initiaize starting sequence
void set_turb_start(bool turb_start) { _heliflags.start_engine = turb_start; }

20
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -608,25 +608,25 @@ void AP_MotorsHeli_Dual::servo_test()
_pitch_in = constrain_float(_pitch_test, -1.0f, 1.0f);
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli_Dual::parameter_check(bool display_msg) const
// Run arming checks
bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const
{
// run base class checks
if (!AP_MotorsHeli::arming_checks(buflen, buffer)) {
return false;
}
// returns false if Phase Angle is outside of range for H3 swashplate 1
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate1.get_phase_angle() > 30 || _swashplate1.get_phase_angle() < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_SW1_H3_PHANG out of range");
}
hal.util->snprintf(buffer, buflen, "H_SW1_H3_PHANG out of range");
return false;
}
// returns false if Phase Angle is outside of range for H3 swashplate 2
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate2.get_phase_angle() > 30 || _swashplate2.get_phase_angle() < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_SW2_H3_PHANG out of range");
}
hal.util->snprintf(buffer, buflen, "H_SW2_H3_PHANG out of range");
return false;
}
// check parent class parameters
return AP_MotorsHeli::parameter_check(display_msg);
return true;
}

4
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -64,8 +64,8 @@ public:
// servo_test - move servos through full range of movement
void servo_test() override;
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
bool parameter_check(bool display_msg) const override;
// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const override;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

28
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -602,41 +602,37 @@ void AP_MotorsHeli_Single::servo_test()
_yaw_in = constrain_float(_yaw_test, -1.0f, 1.0f);
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const
// Run arming checks
bool AP_MotorsHeli_Single::arming_checks(size_t buflen, char *buffer) const
{
// run base class checks
if (!AP_MotorsHeli::arming_checks(buflen, buffer)) {
return false;
}
// returns false if direct drive tailspeed is outside of range
if ((_direct_drive_tailspeed < 0) || (_direct_drive_tailspeed > 100)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_TAIL_SPEED out of range");
}
hal.util->snprintf(buffer, buflen, "H_TAIL_SPEED out of range");
return false;
}
// returns false if Phase Angle is outside of range for H3 swashplate
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate.get_phase_angle() > 30 || _swashplate.get_phase_angle() < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_H3_PHANG out of range");
}
hal.util->snprintf(buffer, buflen, "H_SW_H3_PHANG out of range");
return false;
}
// returns false if Acro External Gyro Gain is outside of range
if ((_ext_gyro_gain_acro < 0) || (_ext_gyro_gain_acro > 1000)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_GYR_GAIN_ACRO out of range");
}
hal.util->snprintf(buffer, buflen, "H_GYR_GAIN_ACRO out of range");
return false;
}
// returns false if Standard External Gyro Gain is outside of range
if ((_ext_gyro_gain_std < 0) || (_ext_gyro_gain_std > 1000)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_GYR_GAIN out of range");
}
hal.util->snprintf(buffer, buflen, "H_GYR_GAIN out of range");
return false;
}
// check parent class parameters
return AP_MotorsHeli::parameter_check(display_msg);
return true;
}

4
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -77,8 +77,8 @@ public:
void set_acro_tail(bool set) override { _acro_tail = set; }
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
bool parameter_check(bool display_msg) const override;
// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const override;
// Thrust Linearization handling
Thrust_Linearization thr_lin {*this};