Rustom Jehangir
9 years ago
committed by
Andrew Tridgell
30 changed files with 58 additions and 908 deletions
@ -1,203 +0,0 @@
@@ -1,203 +0,0 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Sub.h" |
||||
|
||||
// Traditional helicopter variables and functions
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME |
||||
|
||||
#ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN |
||||
#define HELI_DYNAMIC_FLIGHT_SPEED_MIN 500 // we are in "dynamic flight" when the speed is over 1m/s for 2 seconds
|
||||
#endif |
||||
|
||||
// counter to control dynamic flight profile
|
||||
static int8_t heli_dynamic_flight_counter; |
||||
|
||||
// heli_init - perform any special initialisation required for the tradheli
|
||||
void Copter::heli_init() |
||||
{ |
||||
/*
|
||||
automatically set H_RSC_MIN and H_RSC_MAX from RC8_MIN and |
||||
RC8_MAX so that when users upgrade from tradheli version 3.3 to |
||||
3.4 they get the same throttle range as in previous versions of |
||||
the code |
||||
*/ |
||||
if (!g.heli_servo_rsc.radio_min.configured()) { |
||||
g.heli_servo_rsc.radio_min.set_and_save(g.rc_8.radio_min.get()); |
||||
} |
||||
if (!g.heli_servo_rsc.radio_max.configured()) { |
||||
g.heli_servo_rsc.radio_max.set_and_save(g.rc_8.radio_max.get()); |
||||
} |
||||
|
||||
// pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up.
|
||||
input_manager.set_use_stab_col(true); |
||||
input_manager.set_stab_col_ramp(1.0); |
||||
} |
||||
|
||||
// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
|
||||
// should be called at 50hz
|
||||
void Copter::check_dynamic_flight(void) |
||||
{ |
||||
if (!motors.armed() || !motors.rotor_runup_complete() || |
||||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) { |
||||
heli_dynamic_flight_counter = 0; |
||||
heli_flags.dynamic_flight = false; |
||||
return; |
||||
} |
||||
|
||||
bool moving = false; |
||||
|
||||
// with GPS lock use inertial nav to determine if we are moving
|
||||
if (position_ok()) { |
||||
// get horizontal velocity
|
||||
float velocity = inertial_nav.get_velocity_xy(); |
||||
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN); |
||||
}else{ |
||||
// with no GPS lock base it on throttle and forward lean angle
|
||||
moving = (motors.get_throttle() > 800.0f || ahrs.pitch_sensor < -1500); |
||||
} |
||||
|
||||
if (!moving && sonar_enabled && sonar.status() == RangeFinder::RangeFinder_Good) { |
||||
// when we are more than 2m from the ground with good
|
||||
// rangefinder lock consider it to be dynamic flight
|
||||
moving = (sonar.distance_cm() > 200); |
||||
} |
||||
|
||||
if (moving) { |
||||
// if moving for 2 seconds, set the dynamic flight flag
|
||||
if (!heli_flags.dynamic_flight) { |
||||
heli_dynamic_flight_counter++; |
||||
if (heli_dynamic_flight_counter >= 100) { |
||||
heli_flags.dynamic_flight = true; |
||||
heli_dynamic_flight_counter = 100; |
||||
} |
||||
} |
||||
}else{ |
||||
// if not moving for 2 seconds, clear the dynamic flight flag
|
||||
if (heli_flags.dynamic_flight) { |
||||
if (heli_dynamic_flight_counter > 0) { |
||||
heli_dynamic_flight_counter--; |
||||
}else{ |
||||
heli_flags.dynamic_flight = false; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
// update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli.
|
||||
// should be run between the rate controller and the servo updates.
|
||||
void Copter::update_heli_control_dynamics(void) |
||||
{ |
||||
static int16_t hover_roll_trim_scalar_slew = 0; |
||||
|
||||
// Use Leaky_I if we are not moving fast
|
||||
attitude_control.use_leaky_i(!heli_flags.dynamic_flight); |
||||
|
||||
if (ap.land_complete || (motors.get_desired_rotor_speed() == 0)){ |
||||
// if we are landed or there is no rotor power demanded, decrement slew scalar
|
||||
hover_roll_trim_scalar_slew--;
|
||||
} else { |
||||
// if we are not landed and motor power is demanded, increment slew scalar
|
||||
hover_roll_trim_scalar_slew++; |
||||
} |
||||
hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, MAIN_LOOP_RATE); |
||||
|
||||
// set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
|
||||
attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/MAIN_LOOP_RATE)); |
||||
} |
||||
|
||||
// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
|
||||
// should be called soon after update_land_detector in main code
|
||||
void Copter::heli_update_landing_swash() |
||||
{ |
||||
switch(control_mode) { |
||||
case ACRO: |
||||
case STABILIZE: |
||||
case DRIFT: |
||||
case SPORT: |
||||
// manual modes always uses full swash range
|
||||
motors.set_collective_for_landing(false); |
||||
break; |
||||
|
||||
case LAND: |
||||
// landing always uses limit swash range
|
||||
motors.set_collective_for_landing(true); |
||||
break; |
||||
|
||||
case RTL: |
||||
if (rtl_state == RTL_Land) { |
||||
motors.set_collective_for_landing(true); |
||||
}else{ |
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); |
||||
} |
||||
break; |
||||
|
||||
case AUTO: |
||||
if (auto_mode == Auto_Land) { |
||||
motors.set_collective_for_landing(true); |
||||
}else{ |
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); |
||||
} |
||||
break; |
||||
|
||||
default: |
||||
// auto and hold use limited swash when landed
|
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
|
||||
void Copter::heli_update_rotor_speed_targets() |
||||
{ |
||||
|
||||
static bool rotor_runup_complete_last = false; |
||||
|
||||
// get rotor control method
|
||||
uint8_t rsc_control_mode = motors.get_rsc_mode(); |
||||
|
||||
rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in); |
||||
|
||||
|
||||
switch (rsc_control_mode) { |
||||
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH: |
||||
// pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
|
||||
if (rsc_control_deglitched > 10) { |
||||
motors.set_interlock(true); |
||||
motors.set_desired_rotor_speed(rsc_control_deglitched); |
||||
} else { |
||||
motors.set_interlock(false); |
||||
motors.set_desired_rotor_speed(0); |
||||
} |
||||
break; |
||||
case ROTOR_CONTROL_MODE_SPEED_SETPOINT: |
||||
case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT: |
||||
case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT: |
||||
// pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
|
||||
// other than being used to create a crude estimate of rotor speed
|
||||
if (rsc_control_deglitched > 0) { |
||||
motors.set_interlock(true); |
||||
motors.set_desired_rotor_speed(motors.get_rsc_setpoint()); |
||||
}else{ |
||||
motors.set_interlock(false); |
||||
motors.set_desired_rotor_speed(0); |
||||
} |
||||
break; |
||||
} |
||||
|
||||
// when rotor_runup_complete changes to true, log event
|
||||
if (!rotor_runup_complete_last && motors.rotor_runup_complete()){ |
||||
Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE); |
||||
} else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){ |
||||
Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL); |
||||
} |
||||
rotor_runup_complete_last = motors.rotor_runup_complete(); |
||||
} |
||||
|
||||
// heli_radio_passthrough send RC inputs direct into motors library for use during manual passthrough for helicopter setup
|
||||
void Copter::heli_radio_passthrough() |
||||
{ |
||||
motors.set_radio_passthrough(channel_roll->control_in, channel_pitch->control_in, channel_throttle->control_in, channel_yaw->control_in); |
||||
} |
||||
|
||||
#endif // FRAME_CONFIG == HELI_FRAME
|
||||
@ -1,98 +0,0 @@
@@ -1,98 +0,0 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Sub.h" |
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME |
||||
/*
|
||||
* heli_control_acro.pde - init and run calls for acro flight mode for trad heli |
||||
*/ |
||||
|
||||
// heli_acro_init - initialise acro controller
|
||||
bool Copter::heli_acro_init(bool ignore_checks) |
||||
{ |
||||
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
|
||||
attitude_control.use_flybar_passthrough(motors.has_flybar(), motors.supports_yaw_passthrough()); |
||||
|
||||
motors.set_acro_tail(true); |
||||
|
||||
// set stab collective false to use full collective pitch range
|
||||
input_manager.set_use_stab_col(false); |
||||
|
||||
// always successfully enter acro
|
||||
return true; |
||||
} |
||||
|
||||
// heli_acro_run - runs the acro controller
|
||||
// should be called at 100hz or more
|
||||
void Copter::heli_acro_run() |
||||
{ |
||||
float target_roll, target_pitch, target_yaw; |
||||
int16_t pilot_throttle_scaled; |
||||
|
||||
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
|
||||
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
|
||||
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
|
||||
// that the servos move in a realistic fashion while disarmed for operational checks.
|
||||
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
|
||||
|
||||
if(!motors.armed()) { |
||||
heli_flags.init_targets_on_arming=true; |
||||
attitude_control.set_yaw_target_to_current_heading(); |
||||
} |
||||
|
||||
if(motors.armed() && heli_flags.init_targets_on_arming) { |
||||
attitude_control.set_yaw_target_to_current_heading(); |
||||
if (motors.rotor_speed_above_critical()) { |
||||
heli_flags.init_targets_on_arming=false; |
||||
} |
||||
}
|
||||
|
||||
// send RC inputs direct into motors library for use during manual passthrough for helicopter setup
|
||||
heli_radio_passthrough(); |
||||
|
||||
if (!motors.has_flybar()){ |
||||
// convert the input to the desired body frame rate
|
||||
get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); |
||||
|
||||
if (motors.supports_yaw_passthrough()) { |
||||
// if the tail on a flybar heli has an external gyro then
|
||||
// also use no deadzone for the yaw control and
|
||||
// pass-through the input direct to output.
|
||||
target_yaw = channel_yaw->pwm_to_angle_dz(0); |
||||
} |
||||
|
||||
// run attitude controller
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); |
||||
}else{ |
||||
/*
|
||||
for fly-bar passthrough use control_in values with no |
||||
deadzone. This gives true pass-through. |
||||
*/ |
||||
float roll_in = channel_roll->pwm_to_angle_dz(0); |
||||
float pitch_in = channel_pitch->pwm_to_angle_dz(0); |
||||
float yaw_in; |
||||
|
||||
if (motors.supports_yaw_passthrough()) { |
||||
// if the tail on a flybar heli has an external gyro then
|
||||
// also use no deadzone for the yaw control and
|
||||
// pass-through the input direct to output.
|
||||
yaw_in = channel_yaw->pwm_to_angle_dz(0); |
||||
} else { |
||||
// if there is no external gyro then run the usual
|
||||
// ACRO_YAW_P gain on the input control, including
|
||||
// deadzone
|
||||
yaw_in = get_pilot_desired_yaw_rate(channel_yaw->control_in); |
||||
} |
||||
|
||||
// run attitude controller
|
||||
attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in); |
||||
} |
||||
|
||||
// get pilot's desired throttle
|
||||
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); |
||||
|
||||
// output pilot's throttle without angle boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); |
||||
} |
||||
|
||||
#endif //HELI_FRAME
|
||||
@ -1,72 +0,0 @@
@@ -1,72 +0,0 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Sub.h" |
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME |
||||
/*
|
||||
* heli_control_stabilize.pde - init and run calls for stabilize flight mode for trad heli |
||||
*/ |
||||
|
||||
// stabilize_init - initialise stabilize controller
|
||||
bool Copter::heli_stabilize_init(bool ignore_checks) |
||||
{ |
||||
// set target altitude to zero for reporting
|
||||
// To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
|
||||
pos_control.set_alt_target(0); |
||||
|
||||
// set stab collective true to use stabilize scaled collective pitch range
|
||||
input_manager.set_use_stab_col(true); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
// stabilize_run - runs the main stabilize controller
|
||||
// should be called at 100hz or more
|
||||
void Copter::heli_stabilize_run() |
||||
{ |
||||
float target_roll, target_pitch; |
||||
float target_yaw_rate; |
||||
int16_t pilot_throttle_scaled; |
||||
|
||||
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
|
||||
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
|
||||
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
|
||||
// that the servos move in a realistic fashion while disarmed for operational checks.
|
||||
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
|
||||
|
||||
if(!motors.armed()) { |
||||
heli_flags.init_targets_on_arming=true; |
||||
attitude_control.set_yaw_target_to_current_heading(); |
||||
} |
||||
|
||||
if(motors.armed() && heli_flags.init_targets_on_arming) { |
||||
attitude_control.set_yaw_target_to_current_heading(); |
||||
if (motors.rotor_speed_above_critical()) { |
||||
heli_flags.init_targets_on_arming=false; |
||||
} |
||||
} |
||||
|
||||
// send RC inputs direct into motors library for use during manual passthrough for helicopter setup
|
||||
heli_radio_passthrough(); |
||||
|
||||
// apply SIMPLE mode transform to pilot inputs
|
||||
update_simple_mode(); |
||||
|
||||
// convert pilot input to lean angles
|
||||
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
|
||||
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); |
||||
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); |
||||
|
||||
// get pilot's desired throttle
|
||||
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); |
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); |
||||
|
||||
// output pilot's throttle - note that TradHeli does not used angle-boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); |
||||
} |
||||
|
||||
#endif //HELI_FRAME
|
||||
Loading…
Reference in new issue