Copter: add hybrid state struct, formatting changes
This commit is contained in:
parent
b5ed23f592
commit
d9c685323a
@ -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 @@
|
||||
#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
|
||||
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()
|
||||
/*
|
||||
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()
|
||||
// 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()
|
||||
}
|
||||
}
|
||||
|
||||
// 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()
|
||||
// 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()
|
||||
}
|
||||
}
|
||||
// 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()
|
||||
}
|
||||
}
|
||||
// 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()
|
||||
// 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()
|
||||
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()
|
||||
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()
|
||||
}
|
||||
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);
|
||||
|
Loading…
Reference in New Issue
Block a user