zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: add hybrid state struct, formatting changes

master
Randy Mackay 11 years ago
parent
b5ed23f592
commit
d9c685323a
1 changed files with 120 additions and 88 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 208
      ArduCopter/control_hybrid.pde

208
ArduCopter/control_hybrid.pde
Unescape View File

@ -1,11 +1,11 @@ @@ -1,11 +1,11 @@
////////////////////////////////////////////////////////////////////////////////
// Hybrid Mode : ST-JD
// flight mode = 12
////////////////////////////////////////////////////////////////////////////////
#define LOITER_DEADBAND 70
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_hybrid.pde - init and run calls for hybrid flight mode
* hybrid tries to improve upon regular loiter by mixing the pilot input with the loiter controller
*/
#define SPEED_0 10
#define NAV_HYBRID 1
#define NAV_NONE 0
#define LOITER_STAB_TIMER 300 // ST-JD : Must be higher than BRAKE_LOIT_MIX_TIMER (twice is a good deal) set it from 100 to 500, the number of centiseconds between loiter engage and getting wind_comp (once loiter stabilized)
#define BRAKE_LOIT_MIX_TIMER 150 // ST-JD : Must be lower than LOITER_STAB_TIMER set it from 100 to 200, the number of centiseconds brake and loiter commands are mixed to make a smooth transition.
#define LOITER_MAN_MIX_TIMER 50 // ST-JD : set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
@ -14,15 +14,19 @@ @@ -14,15 +14,19 @@
#define THROTTLE_HYBRID_AH 1
#define THROTTLE_HYBRID_BK 3
//#define MX1HYBRID // Alt Hold when throttle in deadband, manual otherwise
#define MX2HYBRID // Alt Hold when throttle from 0 to deadband_high, manual otherwise (above deadband)
//#define MXHYBRID // switch Alt Hold <-> Throttle Assist
//define AHHYBRID // only Alt Hold
//define TAHYBRID // only Throttle Assist
//define JDHYBRID // stick at center=vertical brake and alt-hold when vertical speed is near zero. Stick out from deadband (+70/-70) manual throttle (throttle assist for both!)
// mission state enumeration
enum hybrid_rp_mode {
HYBRID_PILOT_OVERRIDE=0,
HYBRID_BRAKE=1,
HYBRID_LOITER=2
};
static struct {
hybrid_rp_mode roll_mode : 2; // roll mode: pilot override, brake or loiter
hybrid_rp_mode pitch_mode : 2; // pitch mode: pilot override, brake or loiter
uint8_t loiter_engaged : 1; // 1 if loiter target has been set and loiter controller is running
} hybrid;
static uint8_t hybrid_mode_roll; // 1=alt_hold; 2=brake 3=loiter
static uint8_t hybrid_mode_pitch; // 1=alt_hold; 2=brake 3=loiter
static int16_t brake_roll = 0,brake_pitch = 0;
static float K_brake;
static uint8_t throttle_mode=THROTTLE_HYBRID_MAN;
@ -39,29 +43,35 @@ static float brake_loiter_mix; // varies from 0 to @@ -39,29 +43,35 @@ static float brake_loiter_mix; // varies from 0 to
static float loiter_man_mix; // varies from 0 to 1, allow a smooth loiter to manual transition
static int16_t loiter_man_timer;
static int8_t update_wind_offset_timer; // update wind_offset decimator (10Hz)
static int8_t hybrid_nav_mode=NAV_NONE; // replace old nav_mode variable
// hybrid_init - initialise hybrid controller
static bool hybrid_init(bool ignore_checks)
{
/*
if (GPS_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// clear pilot's feed forward for roll and pitch
wp_nav.set_pilot_desired_acceleration(0, 0);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// compute K_brake
K_brake=(15.0f*(float)wp_nav._brake_rate+95.0f)/100.0f;
if (ap.land_complete) {
// Loiter start
hybrid_mode_roll=3;
hybrid_mode_pitch=3;
hybrid.roll_mode = HYBRID_LOITER;
hybrid.pitch_mode = HYBRID_LOITER;
}else{
// Alt_hold like to avoid hard twitch if hybrid enabled in flight
hybrid_mode_roll=1;
hybrid_mode_pitch=1;
hybrid.roll_mode = HYBRID_PILOT_OVERRIDE;
hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE;
}
wind_comp_x=wind_comp_y=0; // Init wind_comp (ef). For now, resetted each time hybrid is switched on
wind_offset_roll=0; // Init offset angles
wind_offset_pitch=0;
@ -90,8 +100,8 @@ static void hybrid_run() @@ -90,8 +100,8 @@ static void hybrid_run()
/*
float target_yaw_rate = 0;
float target_climb_rate = 0;
Vector3f vel; // ST-JD : Used for Hybrid_mode
float vel_fw, vel_right; // ST-JD : Used for Hybrid_mode
const Vector3f& vel = inertial_nav.get_velocity();
float vel_fw, vel_right;
int16_t target_roll, target_pitch;
int16_t pilot_throttle_scaled=0;
@ -109,10 +119,6 @@ static void hybrid_run() @@ -109,10 +119,6 @@ static void hybrid_run()
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
// To-Do: do we need to clear out feed forward if this is not called?
wp_nav.set_pilot_desired_acceleration(0, 0);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
@ -128,45 +134,55 @@ static void hybrid_run() @@ -128,45 +134,55 @@ static void hybrid_run()
}
}
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.set_throttle_out(0, false);
return;
}else{
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
// speed
vel = inertial_nav.get_velocity();
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); // bf -> ef
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); // bf -> ef
// define roll/pitch modes from stick input
// get roll stick input and update new roll mode
if (abs(target_roll) > LOITER_DEADBAND) { //stick input detected => direct to stab mode
hybrid_mode_roll = 1; // Set stab roll mode
}else{
if((hybrid_mode_roll == 1) && (abs(brake_roll) < 2*wp_nav._brake_rate)){ // stick released from stab and copter horizontal (at wind comp) => transition mode
hybrid_mode_roll = 2; // Set brake roll mode
// calculate earth-frame velocities
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// get roll stick input
if (target_roll != 0) {
// roll stick input detected set roll mode to pilot override
hybrid.roll_mode = HYBRID_PILOT_OVERRIDE; // Set stab roll mode
} else {
// stick released from stab and copter horizontal (at wind comp) => transition mode
if ((hybrid.roll_mode == HYBRID_PILOT_OVERRIDE) && (abs(brake_roll) < 2*wp_nav._brake_rate)) {
hybrid.roll_mode = HYBRID_BRAKE; // Set brake roll mode
brake_roll = 0; // this avoid false brake_timeout computing
timeout_roll = 600; // seconds*0.01 - time allowed for the braking to complete, updated at half-braking
timeout_roll_updated = false; // Allow the timeout to be updated only once
brake_max_roll = 0; // used to detect half braking
}else{ // manage brake-to-loiter transition
} else { // manage brake-to-loiter transition
// brake timeout
if (timeout_roll>0) timeout_roll--;
if (timeout_roll > 0) {
timeout_roll--;
}
// Changed loiter engage : not once speed_0 reached but after a little delay that let the copter stabilize if it remains some rate. (maybe compare omega.x/y rather)
if ((fabs(vel_right)<SPEED_0) && (timeout_roll>50)) timeout_roll = 50; // let 0.5s between brake reaches speed_0 and loiter engage
if ((hybrid_mode_roll == 2) && (timeout_roll==0)){ //stick released and transition finished (speed 0) or brake timeout => loiter mode
hybrid_mode_roll = 3; // Set loiter roll mode
if ((fabs(vel_right) < SPEED_0) && (timeout_roll>50)) {
timeout_roll = 50; // let 0.5s between brake reaches speed_0 and loiter engage
}
if ((hybrid.roll_mode == HYBRID_BRAKE) && (timeout_roll==0)){ //stick released and transition finished (speed 0) or brake timeout => loiter mode
hybrid.roll_mode = HYBRID_LOITER; // Set loiter roll mode
}
}
}
//get pitch stick input and update new pitch mode
if (abs(target_pitch) > LOITER_DEADBAND){ //stick input detected => direct to stab mode
hybrid_mode_pitch = 1; // Set stab pitch mode
// get pitch stick input
if (target_pitch != 0) {
// pitch stick input detected set pitch mode to pilot override
hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE; // Set stab pitch mode
}else{
if((hybrid_mode_pitch == 1) && (abs(brake_pitch) < 2*wp_nav._brake_rate)){ // stick released from stab and copter horizontal (at wind_comp) => transition mode
hybrid_mode_pitch = 2; // Set brake pitch mode
if((hybrid.pitch_mode == HYBRID_PILOT_OVERRIDE) && (abs(brake_pitch) < 2*wp_nav._brake_rate)){ // stick released from stab and copter horizontal (at wind_comp) => transition mode
hybrid.pitch_mode = HYBRID_BRAKE; // Set brake pitch mode
brake_pitch = 0; // this avoid false brake_timeout computing
timeout_pitch=600; // seconds*0.01 - time allowed for the braking to complete, updated at half-braking
timeout_pitch_updated = false; // Allow the timeout to be updated only once
@ -175,27 +191,36 @@ static void hybrid_run() @@ -175,27 +191,36 @@ static void hybrid_run()
// brake timeout
if (timeout_pitch>0) timeout_pitch--;
// Changed loiter engage : not once speed_0 reached but after a little delay that let the copter stabilize if it remains some rate. (maybe compare omega.x/y rather)
if((fabs(vel_fw)<SPEED_0) && (timeout_pitch>50)) timeout_pitch = 50; // let 0.5s between brake reaches speed_0 and loiter engage
if ((hybrid_mode_pitch == 2) && (timeout_pitch==0)) {
hybrid_mode_pitch = 3; // Set loiter pitch mode
if ((fabs(vel_fw)<SPEED_0) && (timeout_pitch>50)) {
timeout_pitch = 50; // let 0.5s between brake reaches speed_0 and loiter engage
}
if ((hybrid.pitch_mode == HYBRID_BRAKE) && (timeout_pitch == 0)) {
hybrid.pitch_mode = HYBRID_LOITER; // Set loiter pitch mode
}
}
}
// manual roll/pitch with smooth decrease filter
// roll
if (hybrid_mode_roll == 1){
if (((long)brake_roll*(long)target_roll>=0)&&(abs(target_roll)<STICK_RELEASE_SMOOTH_ANGLE)){ //Smooth decrease only when we want to stop, not if we have to quickly change direction
if (brake_roll>0){ // we use brake_roll to save mem usage and also because it will be natural transition with brake mode.
brake_roll-=max((float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate); //rate decrease
brake_roll=max(brake_roll,target_roll); // use the max value if we increase and because we could have a smoother manual decrease than this computed value
if (hybrid.roll_mode == HYBRID_PILOT_OVERRIDE) {
if (((long)brake_roll*(long)target_roll>=0) && (abs(target_roll)<STICK_RELEASE_SMOOTH_ANGLE)) {
// Smooth decrease only when we want to stop, not if we have to quickly change direction
if (brake_roll > 0){ // we use brake_roll to save mem usage and also because it will be natural transition with brake mode.
// rate decrease
brake_roll -= max((float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate);
// use the max value if we increase and because we could have a smoother manual decrease than this computed value
brake_roll = max(brake_roll,target_roll);
}else{
brake_roll+=max(-(float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate);
brake_roll=min(brake_roll,target_roll);
brake_roll += max(-(float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate);
brake_roll = min(brake_roll,target_roll);
}
}else brake_roll=target_roll;
} else {
brake_roll=target_roll;
}
}
// pitch
if (hybrid_mode_pitch == 1) {
if (hybrid.pitch_mode == HYBRID_PILOT_OVERRIDE) {
if (((long)brake_pitch*(long)target_pitch>=0)&&(abs(target_pitch)<STICK_RELEASE_SMOOTH_ANGLE)){ //Smooth decrease only when we want to stop, not if we have to quickly change direction
if (brake_pitch>0){ // we use brake_pitch to save mem usage and also because it will be natural transition with brake mode.
brake_pitch-=max((float)brake_pitch*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate); //rate decrease
@ -209,21 +234,21 @@ static void hybrid_run() @@ -209,21 +234,21 @@ static void hybrid_run()
}
}
// braking update: roll
if (hybrid_mode_roll>=2) { // Roll: allow braking update to run also during loiter
if (vel_right>=0) { // negative roll = go left, positive roll = go right
brake_roll = max(brake_roll-wp_nav._brake_rate,max((-K_brake*vel_right*(1.0f+500.0f/(vel_right+60.0f))),-wp_nav._max_braking_angle)); // centidegrees
if (hybrid.roll_mode >= HYBRID_BRAKE) { // Roll: allow braking update to run also during loiter
if (vel_right >= 0) { // negative roll = go left, positive roll = go right
brake_roll = max(brake_roll-wp_nav._brake_rate, max((-K_brake*vel_right*(1.0f+500.0f/(vel_right+60.0f))),-wp_nav._max_braking_angle)); // centidegrees
}else{
brake_roll = min(brake_roll+wp_nav._brake_rate,min((-K_brake*vel_right*(1.0f+500.0f/(-vel_right+60.0f))),wp_nav._max_braking_angle)); // centidegrees
brake_roll = min(brake_roll+wp_nav._brake_rate, min((-K_brake*vel_right*(1.0f+500.0f/(-vel_right+60.0f))),wp_nav._max_braking_angle)); // centidegrees
}
if (abs(brake_roll)>brake_max_roll) { // detect half braking and update timeout
brake_max_roll=abs(brake_roll);
if (abs(brake_roll) > brake_max_roll) { // detect half braking and update timeout
brake_max_roll = abs(brake_roll);
} else if (!timeout_roll_updated){
timeout_roll = 1+(uint16_t)(15L*(long)(abs(brake_roll))/(10L*(long)wp_nav._brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
timeout_roll_updated = true;
}
}
// braking update: pitch
if (hybrid_mode_pitch>=2) { // Pitch: allow braking update to run also during loiter
if (hybrid.pitch_mode>=HYBRID_BRAKE) { // Pitch: allow braking update to run also during loiter
if (vel_fw>=0) { // positive pitch = go backward, negative pitch = go forward
brake_pitch = min(brake_pitch+wp_nav._brake_rate,min((K_brake*vel_fw*(1.0f+(500.0f/(vel_fw+60.0f)))),wp_nav._max_braking_angle)); // centidegrees
} else {
@ -238,16 +263,16 @@ static void hybrid_run() @@ -238,16 +263,16 @@ static void hybrid_run()
}
}
// loiter to manual mix
if ((hybrid_mode_pitch==1)||(hybrid_mode_roll==1)) {
if ((hybrid.pitch_mode==HYBRID_PILOT_OVERRIDE)||(hybrid.roll_mode==HYBRID_PILOT_OVERRIDE)) {
if (!ap.land_complete && loiter_man_timer!=0) {
loiter_man_mix = constrain_float((float)(loiter_man_timer)/(float)LOITER_MAN_MIX_TIMER, 0, 1.0);//constrain_float((float)(LOITER_MAN_MIX_TIMER-loiter_man_timer)/(float)LOITER_MAN_MIX_TIMER, 0, 1.0);
loiter_man_timer--;
}
}
// loitering/moving:
if ((hybrid_mode_pitch==3)&&(hybrid_mode_roll==3)){
if (hybrid.pitch_mode == HYBRID_LOITER && hybrid.roll_mode == HYBRID_LOITER) {
// while loitering, updates average lat/lon wind offset angles from I terms
if (hybrid_nav_mode==NAV_HYBRID){
if (hybrid.loiter_engaged) {
if (!ap.land_complete && loiter_stab_timer!=0) {
loiter_stab_timer--;
} else if (max(fabs(vel.x),fabs(vel.y))<SPEED_0) { //Or maybe 2*, 3* speed_0...
@ -257,7 +282,7 @@ static void hybrid_run() @@ -257,7 +282,7 @@ static void hybrid_run()
// Brake_Loiter commands mix factor
brake_loiter_mix = constrain_float((float)(LOITER_STAB_TIMER-loiter_stab_timer)/(float)BRAKE_LOIT_MIX_TIMER, 0, 1.0);
} else {
hybrid_nav_mode=NAV_HYBRID; // turns on NAV_HYBRID if both sticks are at rest
hybrid.loiter_engaged = true; // turns on NAV_HYBRID if both sticks are at rest
pos_control.init_I=false; // restore previous i_terms in Reset_I() => to avoid the stop_and_go effect
wp_nav.init_loiter_target(); // init loiter controller and sets XY stopping point
pos_control.set_target_to_stopping_point_z(); // init altitude
@ -266,8 +291,9 @@ static void hybrid_run() @@ -266,8 +291,9 @@ static void hybrid_run()
brake_pitch = 1; // required for next mode_1 smooth stick release and to avoid twitch
}
} else {
if (hybrid_nav_mode!=NAV_NONE) { // transition from Loiter to Manual
hybrid_nav_mode=NAV_NONE;
// transition from Loiter to Manual
if (hybrid.loiter_engaged) {
hybrid.loiter_engaged = false;
loiter_man_timer=LOITER_MAN_MIX_TIMER;
// save pitch/roll at loiter exit
loiter_roll=wp_nav.get_roll();
@ -279,43 +305,45 @@ static void hybrid_run() @@ -279,43 +305,45 @@ static void hybrid_run()
wind_offset_pitch = (float)fast_atan(-(wind_comp_x*ahrs.cos_yaw() + wind_comp_y*ahrs.sin_yaw())/981)*(18000/M_PI);
wind_offset_roll = (float)fast_atan((-wind_comp_x*ahrs.sin_yaw() + wind_comp_y*ahrs.cos_yaw())/981)*(18000/M_PI);
update_wind_offset_timer=10;
} else update_wind_offset_timer--;
} else {
update_wind_offset_timer--;
}
}
// if required, update loiter controller
if(hybrid_nav_mode == NAV_HYBRID) {
if(hybrid.loiter_engaged) {
wp_nav.update_loiter();
}
// select output to stabilize controllers
switch (hybrid_mode_roll) {
switch (hybrid.roll_mode) {
// To-Do: try to mix loiter->manual using brake_loiter_mix variable as we are doing on loiter engage
case 1:
case HYBRID_PILOT_OVERRIDE:
// Loiter-Manual mix at loiter exit
target_roll = loiter_man_mix*(float)loiter_roll+(1.0f-loiter_man_mix)*(float)(brake_roll+wind_offset_roll);
break;
case 2:
target_roll = brake_roll+wind_offset_roll;
case HYBRID_BRAKE:
target_roll = brake_roll + wind_offset_roll;
break;
case 3:
if (hybrid_nav_mode == NAV_HYBRID) { // if nav_hybrid enabled...
case HYBRID_LOITER:
if (hybrid.loiter_engaged) { // if nav_hybrid enabled...
// Brake_Loiter mix at loiter engage
target_roll = brake_loiter_mix*(float)wp_nav.get_roll()+(1.0f-brake_loiter_mix)*(float)(brake_roll+wind_offset_roll);
}else {
target_roll = brake_roll+wind_offset_roll;
target_roll = brake_roll + wind_offset_roll;
}
break;
}
switch (hybrid_mode_pitch){
case 1:
switch (hybrid.pitch_mode){
case HYBRID_PILOT_OVERRIDE:
//Loiter-Manual mix at loiter exit
target_pitch = loiter_man_mix*(float)loiter_pitch+(1.0f-loiter_man_mix)*(float)(brake_pitch+wind_offset_pitch);
break;
case 2:
case HYBRID_BRAKE:
target_pitch = brake_pitch+wind_offset_pitch;
break;
case 3:
if(hybrid_nav_mode == NAV_HYBRID) { // if nav_hybrid enabled...
case HYBRID_LOITER:
if(hybrid.loiter_engaged) { // if nav_hybrid enabled...
// Brake_Loiter mix at loiter engage
target_pitch = brake_loiter_mix*(float)wp_nav.get_pitch()+(1.0f-brake_loiter_mix)*(float)(brake_pitch+wind_offset_pitch);
} else {
@ -323,11 +351,15 @@ static void hybrid_run() @@ -323,11 +351,15 @@ static void hybrid_run()
}
break;
}
// clip target pitch/roll
// constrain target pitch/roll angles
target_roll=constrain_int16(target_roll,-aparm.angle_max,aparm.angle_max);
target_pitch=constrain_int16(target_pitch,-aparm.angle_max,aparm.angle_max);
// update attitude controller targets
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
// throttle control
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);

Write Preview
Loading…
Cancel
Save