Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ACM : Attitude.pde

Browse Source 
yaw limit, moved toy code out of att to it's own pre
This commit is contained in:
Jason Short 2012-08-09 16:45:47 -07:00
parent 7615c835ee
commit c3f01bebfc
1 changed files with 10 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
ArduCopter/Attitude.pde
View File

@ -338,8 +338,16 @@ get_rate_yaw(int32_t target_rate)
}
#endif
// constrain output
return output;
#if FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME
// constrain output
return output;
#else
// output control:
int16_t yaw_limit = 2000 + abs(g.rc_4.control_in);
// smoother Yaw control:
return constrain(output, -yaw_limit, yaw_limit);
#endif
}
static int16_t
@ -773,128 +781,3 @@ get_of_pitch(int32_t control_pitch)
return control_pitch;
#endif
}
// THOR
// The function call for managing the flight mode Toy
void roll_pitch_toy()
{
bool manual_control = false;
if(g.rc_2.control_in != 0){
// If we pitch forward or back, resume manually control
manual_control = true;
}
// Yaw control - Yaw is always available, and will NOT exit the
// user from Loiter mode
int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
if(g.rc_1.control_in != 0){ // roll
g.rc_4.servo_out = get_acro_yaw(yaw_rate/2);
yaw_stopped = false;
yaw_timer = 150;
}else if (!yaw_stopped){
g.rc_4.servo_out = get_acro_yaw(0);
yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)){
yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;
}
}else{
if(motors.armed() == false)
nav_yaw = ahrs.yaw_sensor;
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
}
if(manual_control){
// user is in control: reset count-up timer
toy_input_timer = 0;
// roll_rate is the outcome of the linear equation or lookup table
// based on speed and Yaw rate
int16_t roll_rate = 0;
// We manually set out modes based on the state of Toy mode:
// Handle throttle manually
throttle_mode = THROTTLE_MANUAL;
// Dont try to navigate or integrate a nav error
wp_control = NO_NAV_MODE;
#if TOY_LOOKUP == 1
uint8_t xx, yy;
// Lookup value
xx = g_gps->ground_speed / 200;
yy = abs(yaw_rate / 500);
// constrain to lookup Array range
xx = constrain(xx, 0, 3);
yy = constrain(yy, 0, 8);
roll_rate = toy_lookup[yy * 4 + xx];
if(yaw_rate == 0)
roll_rate = 0;
else if(yaw_rate < 0)
roll_rate = -roll_rate;
int16_t roll_limit = 4500 / g.toy_yaw_rate;
roll_rate = constrain(roll_rate, -roll_limit, roll_limit);
#else
// yaw_rate = roll angle
// Linear equation for Yaw:Speed to Roll
// default is 1000, lower for more roll action
//roll_rate = ((float)g_gps->ground_speed / 600) * (float)yaw_rate;
roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
//Serial.printf("roll_rate: %d\n",roll_rate);
// limit roll rate to 15, 30, or 45 deg per second.
//int16_t roll_limit = 4500 / g.toy_yaw_rate;
//roll_rate = constrain(roll_rate, -roll_limit, roll_limit);
roll_rate = constrain(roll_rate, -2500, 2500);
//Serial.printf("yaw_rate %d, roll_rate %d, lim %d\n",yaw_rate, roll_rate, roll_limit);
#endif
// Output the attitude
g.rc_1.servo_out = get_stabilize_roll(roll_rate);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
}else{
//no user input
// Count-up to decision tp Loiter
toy_input_timer++;
//if (toy_input_timer == TOY_DELAY){
if((wp_control != LOITER_MODE) && ((g_gps->ground_speed < 150) || (toy_input_timer == TOY_DELAY))){
// clear our I terms for Nav or we will carry over old values
reset_wind_I();
// loiter
wp_control = LOITER_MODE;
// we are in an alt hold throttle with manual override
throttle_mode = THROTTLE_HOLD;
set_next_WP(&current_loc);
}
if (wp_control == LOITER_MODE){
// prevent timer overflow
toy_input_timer = TOY_DELAY;
// outputs the needed nav_control to maintain speed and direction
g.rc_1.servo_out = get_stabilize_roll(auto_roll);
g.rc_2.servo_out = get_stabilize_pitch(auto_pitch);
}else{
// Coast
g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_2.servo_out = get_stabilize_pitch(0);
}
}
}