mirror of https://github.com/ArduPilot/ardupilot
TradHeli: integrate motor lib changes
This commit is contained in:
parent
13a412ee21
commit
46118b59d7
|
@ -1626,7 +1626,7 @@ void update_roll_pitch_mode(void)
|
|||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// ACRO does not get SIMPLE mode ability
|
||||
if (motors.flybar_mode == 1) {
|
||||
if (motors.has_flybar()) {
|
||||
g.rc_1.servo_out = g.rc_1.control_in;
|
||||
g.rc_2.servo_out = g.rc_2.control_in;
|
||||
}else{
|
||||
|
@ -1850,6 +1850,11 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
|
|||
// reset some variables used for logging
|
||||
desired_climb_rate = 0;
|
||||
nav_throttle = 0;
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// use reduced collective range if in manual throttle mode
|
||||
motors.set_collective_for_manual_control(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED);
|
||||
#endif
|
||||
}
|
||||
|
||||
// return success or failure
|
||||
|
@ -1867,13 +1872,9 @@ void update_throttle_mode(void)
|
|||
return;
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
||||
if (control_mode == STABILIZE){
|
||||
motors.stab_throttle = true;
|
||||
} else {
|
||||
motors.stab_throttle = false;
|
||||
}
|
||||
// check if we are flying
|
||||
check_dynamic_flight();
|
||||
|
||||
// allow swash collective to move if we are in manual throttle modes, even if disarmed
|
||||
if( !motors.armed() ) {
|
||||
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
|
||||
|
@ -1881,7 +1882,6 @@ void update_throttle_mode(void)
|
|||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#else // HELI_FRAME
|
||||
|
||||
// do not run throttle controllers if motors disarmed
|
||||
|
@ -1907,7 +1907,7 @@ void update_throttle_mode(void)
|
|||
|
||||
// update estimate of throttle cruise
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
update_throttle_cruise(motors.coll_out);
|
||||
update_throttle_cruise(motors.get_collective_out());
|
||||
#else
|
||||
update_throttle_cruise(pilot_throttle_scaled);
|
||||
// check if we've taken off yet
|
||||
|
@ -1932,7 +1932,7 @@ void update_throttle_mode(void)
|
|||
|
||||
// update estimate of throttle cruise
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
update_throttle_cruise(motors.coll_out);
|
||||
update_throttle_cruise(motors.get_collective_out());
|
||||
#else
|
||||
update_throttle_cruise(pilot_throttle_scaled);
|
||||
if (!ap.takeoff_complete && motors.armed()) {
|
||||
|
@ -1997,7 +1997,7 @@ void update_throttle_mode(void)
|
|||
}else{
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// collective pitch should not be full negative to avoid harshing the mechanics. Use Stab Coll Min.
|
||||
set_throttle_out(motors.stab_col_min*10, false);
|
||||
set_throttle_out(motors.get_manual_collective_min(), false);
|
||||
#else
|
||||
// pilot's throttle must be at zero so keep motors off
|
||||
set_throttle_out(0, false);
|
||||
|
@ -2212,7 +2212,7 @@ static void tuning(){
|
|||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
case CH6_HELI_EXTERNAL_GYRO:
|
||||
motors.ext_gyro_gain = tuning_value;
|
||||
motors.ext_gyro_gain(tuning_value);
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
|
@ -83,7 +83,7 @@ get_stabilize_yaw(int32_t target_angle)
|
|||
|
||||
// do not use rate controllers for helicotpers with external gyros
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if(motors.ext_gyro_enabled) {
|
||||
if(motors.ext_gyro_enabled()) {
|
||||
g.rc_4.servo_out = constrain_int32(target_rate, -4500, 4500);
|
||||
}
|
||||
#endif
|
||||
|
@ -294,7 +294,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
|
|||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if (!motors.motor_runup_complete) {
|
||||
if (!motors.motor_runup_complete()) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#else
|
||||
|
@ -335,7 +335,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
|
|||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if (!motors.motor_runup_complete) {
|
||||
if (!motors.motor_runup_complete()) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#else
|
||||
|
@ -373,7 +373,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
|
|||
angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if (!motors.motor_runup_complete) {
|
||||
if (!motors.motor_runup_complete()) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#else
|
||||
|
@ -443,7 +443,7 @@ void
|
|||
run_rate_controllers()
|
||||
{
|
||||
#if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw and only when not using an external gyro
|
||||
if(!motors.ext_gyro_enabled) {
|
||||
if(!motors.ext_gyro_enabled()) {
|
||||
heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf);
|
||||
g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf);
|
||||
}
|
||||
|
@ -780,7 +780,7 @@ static int16_t get_angle_boost(int16_t throttle)
|
|||
{
|
||||
float angle_boost_factor = cos_pitch_x * cos_roll_x;
|
||||
angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f);
|
||||
int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0);
|
||||
int16_t throttle_above_mid = max(throttle - motors.get_collective_mid(),0);
|
||||
|
||||
// to allow logging of angle boost
|
||||
angle_boost = throttle_above_mid*angle_boost_factor;
|
||||
|
@ -900,7 +900,7 @@ get_throttle_accel(int16_t z_target_accel)
|
|||
} else {
|
||||
// Avoid harshing the mechanics pushing into the ground
|
||||
// stab_col_min should gently push down on the ground
|
||||
output = constrain_float(p+i+d+g.throttle_cruise, motors.stab_col_min*10, motors.stab_col_max*10);
|
||||
output = constrain_float(p+i+d+g.throttle_cruise, motors.get_manual_collective_min(), motors.get_manual_collective_max());
|
||||
}
|
||||
#else
|
||||
output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
|
||||
|
|
|
@ -271,7 +271,7 @@ static void Log_Write_Motors()
|
|||
motors.motor_out[AP_MOTORS_MOT_2],
|
||||
motors.motor_out[AP_MOTORS_MOT_3],
|
||||
motors.motor_out[AP_MOTORS_MOT_4]},
|
||||
ext_gyro_gain : motors.ext_gyro_gain
|
||||
ext_gyro_gain : motors.ext_gyro_gain()
|
||||
#elif FRAME_CONFIG == TRI_FRAME
|
||||
motor_out : {motors.motor_out[AP_MOTORS_MOT_1],
|
||||
motors.motor_out[AP_MOTORS_MOT_2],
|
||||
|
|
|
@ -20,7 +20,7 @@ static struct {
|
|||
// should be called at 50hz
|
||||
static void check_dynamic_flight(void)
|
||||
{
|
||||
if (!motors.armed() || throttle_mode == THROTTLE_LAND || !motors.motor_runup_complete) {
|
||||
if (!motors.armed() || throttle_mode == THROTTLE_LAND || !motors.motor_runup_complete()) {
|
||||
heli_dynamic_flight_counter = 0;
|
||||
heli_flags.dynamic_flight = false;
|
||||
return;
|
||||
|
@ -89,7 +89,7 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
|
|||
if (roll_pid_saturated){
|
||||
roll_i = g.pid_rate_roll.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
|
||||
} else {
|
||||
if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
|
||||
if (motors.has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
|
||||
if (target_roll_rate > -50 && target_roll_rate < 50){ // Frozen at high rates
|
||||
roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
|
||||
} else {
|
||||
|
@ -107,7 +107,7 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
|
|||
if (pitch_pid_saturated){
|
||||
pitch_i = g.pid_rate_pitch.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
|
||||
} else {
|
||||
if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
|
||||
if (motors.has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
|
||||
if (target_pitch_rate > -50 && target_pitch_rate < 50){ // Frozen at high rates
|
||||
pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
|
||||
} else {
|
||||
|
|
|
@ -34,12 +34,13 @@ static void arm_motors_check()
|
|||
return;
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if ((motors.rsc_mode > 0) && (g.rc_8.control_in >= 10)){
|
||||
arming_counter = 0;
|
||||
return;
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// heli specific arming check
|
||||
if (!motors.allow_arming()){
|
||||
arming_counter = 0;
|
||||
return;
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
||||
int16_t tmp = g.rc_4.control_in;
|
||||
|
||||
|
|
|
@ -170,11 +170,6 @@ static void init_ardupilot()
|
|||
}
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
motors.servo_manual = false;
|
||||
motors.init_swash(); // heli initialisation
|
||||
#endif
|
||||
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
init_rc_out(); // sets up motors and output to escs
|
||||
|
||||
|
@ -498,13 +493,15 @@ static void update_auto_armed()
|
|||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
|
||||
if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete) {
|
||||
if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete()) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
#else
|
||||
// if motors are armed and throttle is above zero auto_armed should be true
|
||||
if(motors.armed() && g.rc_3.control_in != 0) {
|
||||
#endif // HELI_FRAME
|
||||
set_auto_armed(true);
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue