Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: added back in angle boost and recombined for heli and multicopter

Browse Source
This commit is contained in:
rmackay9 2012-11-24 13:41:17 +09:00
parent c1ce0ae752
commit 1285198b71
3 changed files with 49 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/ArduCopter.pde
View File

@ -1680,7 +1680,7 @@ void update_throttle_mode(void)
// do not run throttle controllers if motors disarmed
if( !motors.armed() ) {
set_throttle_out(0);
set_throttle_out(0, false);
return;
}
@ -1697,10 +1697,10 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL:
// completely manual throttle
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
set_throttle_out(0, false);
}else{
// send pilot's output directly to motors
set_throttle_out(g.rc_3.control_in);
set_throttle_out(g.rc_3.control_in, false);
// update estimate of throttle cruise
update_throttle_cruise(g.rc_3.control_in);
@ -1718,15 +1718,9 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL_TILT_COMPENSATED:
// manual throttle but with angle boost
if (g.rc_3.control_in <= 0) {
set_throttle_out(0);
set_throttle_out(0, false); // no need for angle boost with zero throttle
}else{
// TO-DO: combine multicopter and tradheli angle boost functions
#if FRAME_CONFIG == HELI_FRAME
set_throttle_out(heli_get_angle_boost(g.rc_3.control_in));
#else
angle_boost = get_angle_boost(g.rc_3.control_in);
set_throttle_out(g.rc_3.control_in + angle_boost);
#endif
set_throttle_out(g.rc_3.control_in, true);
// update estimate of throttle cruise
update_throttle_cruise(g.rc_3.control_in);
@ -1743,16 +1737,17 @@ void update_throttle_mode(void)
case THROTTLE_ACCELERATION:
// pilot inputs the desired acceleration
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
set_throttle_out(0, false);
}else{
int16_t desired_acceleration = get_pilot_desired_acceleration(g.rc_3.control_in);
set_throttle_accel_target(desired_acceleration);
}
break;
case THROTTLE_RATE:
// pilot inputs the desired climb rate. Note this is the unstabilized rate controller
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
set_throttle_out(0, false);
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate(pilot_climb_rate);
@ -1762,7 +1757,7 @@ void update_throttle_mode(void)
case THROTTLE_STABILIZED_RATE:
// pilot inputs the desired climb rate. Note this is the unstabilized rate controller
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
set_throttle_out(0, false);
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate_stabilized(pilot_climb_rate);
@ -1772,7 +1767,7 @@ void update_throttle_mode(void)
case THROTTLE_DIRECT_ALT:
// pilot inputs a desired altitude from 0 ~ 10 meters
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
set_throttle_out(0, false);
}else{
// To-Do: this should update the global desired altitude variable next_WP.alt
int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in);
@ -1783,14 +1778,7 @@ void update_throttle_mode(void)
case THROTTLE_HOLD:
// alt hold plus pilot input of climb rate
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
// check for pilot override
if( pilot_climb_rate != 0 ) {
get_throttle_rate_stabilized(pilot_climb_rate);
}else{
get_throttle_rate_stabilized(0);
force_new_altitude(current_loc.alt); //TO-DO: this should be set to stabilized target
}
break;
case THROTTLE_AUTO:

48
ArduCopter/Attitude.pde
View File

@ -294,7 +294,7 @@ run_rate_controllers()
// run throttle controller if an accel based target has been provided
if( throttle_accel_controller_active ) {
g.rc_3.servo_out = get_throttle_accel(throttle_accel_target_ef);
set_throttle_out(get_throttle_accel(throttle_accel_target_ef), true);
}
}
@ -718,32 +718,52 @@ static void update_throttle_cruise(int16_t throttle)
}
}
static int16_t get_angle_boost(int16_t value)
{
float temp = cos_pitch_x * cos_roll_x;
temp = constrain(temp, .75, 1.0);
return ((float)(value + 80) / temp) - (value + 80);
}
#if FRAME_CONFIG == HELI_FRAME
// heli_angle_boost - adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
static int16_t heli_get_angle_boost(int16_t throttle)
// for traditional helicopters
static int16_t 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);
int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0);
return throttle + throttle_above_mid*angle_boost_factor;
// to allow logging of angle boost
angle_boost = throttle_above_mid*angle_boost_factor;
return throttle + angle_boost;
}
#endif // HELI_FRAME
#else // all multicopters
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
static int16_t get_angle_boost(int16_t throttle)
{
float temp = cos_pitch_x * cos_roll_x;
int16_t throttle_out;
temp = constrain(temp, .5, 1.0);
temp = constrain(9000-max(labs(roll_axis),labs(pitch_axis)), 0, 3000) / (3000 * temp);
throttle_out = constrain((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000);
//Serial.printf("Thin:%4.2f sincos:%4.2f temp:%4.2f roll_axis:%4.2f Out:%4.2f \n", 1.0*throttle, 1.0*cos_pitch_x * cos_roll_x, 1.0*temp, 1.0*roll_axis, 1.0*constrain((float)value * temp, 0, 1000));
// to allow logging of angle boost
angle_boost = throttle_out - throttle;
return throttle_out;
}
#endif // FRAME_CONFIG == HELI_FRAME
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors (instead of using accel based throttle controller)
// provide 0 to cut motors
void set_throttle_out( int16_t throttle_out )
void set_throttle_out( int16_t throttle_out, bool apply_angle_boost )
{
if( apply_angle_boost ) {
g.rc_3.servo_out = get_angle_boost(throttle_out);
}else{
g.rc_3.servo_out = throttle_out;
// clear angle_boost for logging purposes
angle_boost = 0;
}
throttle_accel_controller_active = false;
}

6
ArduCopter/flip.pde
View File

@ -39,7 +39,7 @@ void roll_flip()
if (roll < 4500) {
// Roll control
g.rc_1.servo_out = AAP_ROLL_OUT * flip_dir;
set_throttle_out(g.rc_3.control_in + AAP_THR_INC);
set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false);
}else{
flip_state++;
}
@ -52,7 +52,7 @@ void roll_flip()
#else
g.rc_1.servo_out = get_rate_roll(40000 * flip_dir);
#endif // HELI_FRAME
set_throttle_out(g.rc_3.control_in - AAP_THR_DEC);
set_throttle_out(g.rc_3.control_in - AAP_THR_DEC, false);
}else{
flip_state++;
flip_timer = 0;
@ -63,7 +63,7 @@ void roll_flip()
if (flip_timer < 100) {
//g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
get_stabilize_roll(g.rc_1.control_in);
set_throttle_out(g.rc_3.control_in + AAP_THR_INC);
set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false);
flip_timer++;
}else{
Log_Write_Event(DATA_END_FLIP);