diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a0ab216d0e..3a852e4429 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -328,6 +328,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist_angle, 30), + // @Param: TILT_TYPE + // @DisplayName: Tiltrotor type + // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE + // @Values: 0:Continuous,1:Binary + AP_GROUPINFO("TILT_TYPE", 46, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS), + AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index a6337cbd35..ae166b6abb 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -299,12 +299,16 @@ private: // time of last control log message uint32_t last_ctrl_log_ms; + + // types of tilt mechanisms + enum {TILT_TYPE_CONTINUOUS=0, TILT_TYPE_BINARY=1}; // tiltrotor control variables struct { AP_Int16 tilt_mask; AP_Int16 max_rate_dps; AP_Int8 max_angle_deg; + AP_Int8 tilt_type; float current_tilt; float current_throttle; bool motors_active:1; @@ -314,7 +318,10 @@ private: uint32_t last_motors_active_ms; void tiltrotor_slew(float tilt); + void tiltrotor_binary_slew(bool forward); void tiltrotor_update(void); + void tiltrotor_continuous_update(void); + void tiltrotor_binary_update(void); void tilt_compensate(float *thrust, uint8_t num_motors); void afs_terminate(void); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index d5309c3643..c30925fd55 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -22,15 +22,10 @@ void QuadPlane::tiltrotor_slew(float newtilt) } /* - update motor tilt + update motor tilt for continuous tilt servos */ -void QuadPlane::tiltrotor_update(void) +void QuadPlane::tiltrotor_continuous_update(void) { - if (tilt.tilt_mask <= 0) { - // no motors to tilt - return; - } - // default to inactive tilt.motors_active = false; @@ -102,6 +97,65 @@ void QuadPlane::tiltrotor_update(void) } +/* + output a slew limited tiltrotor angle. tilt is 0 or 1 + */ +void QuadPlane::tiltrotor_binary_slew(bool forward) +{ + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_motor_tilt, forward?1000:0); + + float max_change = (tilt.max_rate_dps.get() * plane.G_Dt) / 90.0f; + if (forward) { + tilt.current_tilt = constrain_float(tilt.current_tilt+max_change, 0, 1); + } else { + tilt.current_tilt = constrain_float(tilt.current_tilt-max_change, 0, 1); + } + + // setup tilt compensation + motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t)); +} + +/* + update motor tilt for binary tilt servos + */ +void QuadPlane::tiltrotor_binary_update(void) +{ + // motors always active + tilt.motors_active = true; + + if (!in_vtol_mode()) { + // we are in pure fixed wing mode. Move the tiltable motors + // all the way forward and run them as a forward motor + tiltrotor_binary_slew(true); + + float new_throttle = plane.channel_throttle->get_servo_out()*0.01f; + if (tilt.current_tilt >= 1) { + // the motors are all the way forward, start using them for fwd thrust + motors->output_motor_mask(new_throttle, (uint8_t)tilt.tilt_mask.get()); + } + } else { + tiltrotor_binary_slew(false); + } +} + + +/* + update motor tilt + */ +void QuadPlane::tiltrotor_update(void) +{ + if (tilt.tilt_mask <= 0) { + // no motors to tilt + return; + } + if (tilt.tilt_type == TILT_TYPE_BINARY) { + tiltrotor_binary_update(); + } else { + tiltrotor_continuous_update(); + } +} + + /* compensate for tilt in a set of motor outputs