ArduCopter - Attitude.pde - moved heli_get_angle_boost from heli.pde (which will be removed) to Attitude.pde. In fact we should combine heli_get_angle_boost and the regular get_angle_boost.

This commit is contained in:
rmackay9 2012-04-04 22:51:39 +09:00
parent 544237d60a
commit 23c3f25654
1 changed files with 15 additions and 1 deletions

View File

@ -80,7 +80,7 @@ get_stabilize_yaw(int32_t target_angle)
// do not use rate controllers for helicotpers with external gyros // do not use rate controllers for helicotpers with external gyros
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if(!g.heli_ext_gyro_enabled){ if(!motors.ext_gyro_enabled){
output = get_rate_yaw(target_rate) + i_term; output = get_rate_yaw(target_rate) + i_term;
}else{ }else{
output = constrain((target_rate + i_term), -4500, 4500); output = constrain((target_rate + i_term), -4500, 4500);
@ -390,6 +390,20 @@ static int16_t get_angle_boost(int16_t value)
// return (int)(temp * value); // return (int)(temp * value);
} }
#if FRAME_CONFIG == HELI_FRAME
// heli_angle_boost - adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// throttle value should be 0 ~ 1000
static int16_t heli_get_angle_boost(int16_t throttle)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
int throttle_above_mid = max(throttle - motors.throttle_mid,0);
return throttle + throttle_above_mid*angle_boost_factor;
}
#endif // HELI_FRAME
#define NUM_G_SAMPLES 40 #define NUM_G_SAMPLES 40
#if ACCEL_ALT_HOLD == 2 #if ACCEL_ALT_HOLD == 2