Skip to content

Commit

Permalink
AP_Motors_Heli: Fix DDFP min-max variables
Browse files Browse the repository at this point in the history
  • Loading branch information
@MattKear
MattKear committed Sep 30, 2023
1 parent 1641aa4 commit b377486
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 12 deletions.
80 changes: 68 additions & 12 deletions libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,6 +196,9 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
// init_outputs - initialise Servo/PWM ranges and endpoints
void AP_MotorsHeli_Single::init_outputs()
{

bool init_ok = _frame_class == MOTOR_FRAME_HELI;

if (!initialised_ok()) {
// yaw servo
add_motor_num(CH_4);
Expand All @@ -221,7 +224,13 @@ void AP_MotorsHeli_Single::init_outputs()
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
}

set_initialised_ok(_frame_class == MOTOR_FRAME_HELI);
// Ensure we keep ddfp range and trim up to date as it is used in thrust linearisation calcs
if (!update_ddfp_range_and_trim()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "DDFP init fail");
init_ok = false;
}

set_initialised_ok(init_ok);
}

// set_desired_rotor_speed
Expand Down Expand Up @@ -490,54 +499,61 @@ void AP_MotorsHeli_Single::output_to_motors()
thr_lin.update_lift_max_from_batt_voltage();
}

// Ensure we keep ddfp range and trim up to date as it is used in thrust linearisation calcs
bool ddfp_configured = update_ddfp_range_and_trim();

switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
uint16_t ddfp_output = 0;
if (ddfp_configured) {
ddfp_output = calculate_ddfp_output(0.0f);
}
rc_write(AP_MOTORS_MOT_4, ddfp_output);
}
break;
case SpoolState::GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
uint16_t ddfp_output = 0;
if (ddfp_configured) {
ddfp_output = calculate_ddfp_output(0.0f);
}
rc_write(AP_MOTORS_MOT_4, ddfp_output);
}
break;
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){
SRV_Channel *c = SRV_Channels::srv_channel(AP_MOTORS_MOT_4);
if (c != nullptr) {
_ddfp_pwm_min = c->get_output_min();
_ddfp_pwm_max = c->get_output_max();
_ddfp_pwm_trim = c->get_trim();
}
float servo_out = 0.0f;
if (is_positive((float) (_ddfp_pwm_max - _ddfp_pwm_min))) {
if (ddfp_configured) {
float servo4_trim = constrain_float((_ddfp_pwm_trim - 1000) / (_ddfp_pwm_max - _ddfp_pwm_min), 0.0f, 1.0f);
if (is_positive(_servo4_out)) {
servo_out = (1.0f - servo4_trim) * _servo4_out + servo4_trim;
} else {
servo_out = servo4_trim * _servo4_out + servo4_trim;
}
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(thr_lin.thrust_to_actuator(constrain_float(servo_out, 0.0f, 1.0f))));
} else {
// if servo pwm min and max are bad, convert servo4_out from -1 to 1 to 0 to 1
servo_out = 0.5f * (_servo4_out + 1.0f);
// write as an angle and let servo channel class handle the trim min max
rc_write_angle(AP_MOTORS_MOT_4, thr_lin.thrust_to_actuator(constrain_float(servo_out, 0.0f, 1.0f))*4500);
// this should never happen
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
// output yaw servo to tail rsc
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(thr_lin.thrust_to_actuator(constrain_float(servo_out, 0.0f, 1.0f))));
}
break;
case SpoolState::SPOOLING_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){
// TODO: add ddfp checks
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
}
break;
Expand All @@ -552,6 +568,46 @@ uint16_t AP_MotorsHeli_Single::calculate_ddfp_output(float yaw_out)
return ret;
}

// if using DDFP tail we need to get the pwm range and trim of the servo channel for use later
bool AP_MotorsHeli_Single::update_ddfp_range_and_trim(void)
{
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
return true;
}

// find the channel that motor 4 is assigned to
uint8_t chan;
if (!SRV_Channels::find_channel(SRV_Channel::k_motor4, chan)) {
// We have failed to find the tail motor channel
gcs().send_text(MAV_SEVERITY_CRITICAL, "DDFP Error: Motor 4 not assigned");
return false;
}

SRV_Channel *c = SRV_Channels::srv_channel(chan);
if (c != nullptr) {
_ddfp_pwm_min = c->get_output_min();
_ddfp_pwm_max = c->get_output_max();
_ddfp_pwm_trim = c->get_trim();
} else {
// this should never happen, somthing has gone very wrong if we hit this
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
}

// error checking
if (_ddfp_pwm_max <= 0 || _ddfp_pwm_min <= 0 || _ddfp_pwm_trim <= 0) {
return false;
}

// sanity checking
if ((_ddfp_pwm_max <= _ddfp_pwm_min) || (_ddfp_pwm_trim < _ddfp_pwm_min) || (_ddfp_pwm_trim > _ddfp_pwm_max)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "DDFP: servo min max trim bad config");
return false;
}

return true;
}

// servo_test - move servos through full range of movement
void AP_MotorsHeli_Single::servo_test()
{
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli_Single.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,9 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
// calculate the motor output for DDFP tails from yaw_out
uint16_t calculate_ddfp_output(float yaw_out);

// helper for keeping output min, max, and trim values on the ddfp output up to date with error checking included
bool update_ddfp_range_and_trim(void);

// servo_test - move servos through full range of movement
void servo_test() override;

Expand Down

0 comments on commit b377486

Please sign in to comment.