dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw pos ctrl: add wrapper function for tecs

This commit is contained in:
Thomas Gubler 2014-03-10 23:37:26 +01:00
parent b6a087e921
commit 313b546c4d
1 changed files with 53 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -356,6 +356,15 @@ private:
* Reset landing state * Reset landing state
*/ */
void reset_landing_state(); void reset_landing_state();
/*
* Call TECS
*/
void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad);
}; };
namespace l1_control namespace l1_control
@ -819,11 +828,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll(); _att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing(); _att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
_airspeed.indicated_airspeed_m_s, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min));
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
@ -833,11 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll(); _att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing(); _att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
_airspeed.indicated_airspeed_m_s, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min));
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
@ -929,11 +936,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_stayonground = true; land_stayonground = true;
} }
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas,
_airspeed.indicated_airspeed_m_s, eas2tas, flare_pitch_angle_rad, math::radians(15.0f),
false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land,
0.0f, throttle_max, throttle_land, false, flare_pitch_angle_rad);
flare_pitch_angle_rad, math::radians(15.0f));
if (!land_noreturn_vertical) { if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@ -962,11 +968,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
altitude_desired = math::max(_global_pos.alt, L_altitude); altitude_desired = math::max(_global_pos.alt, L_altitude);
} }
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas,
_airspeed.indicated_airspeed_m_s, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min));
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} }
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
@ -1004,22 +1009,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (altitude_error > 15.0f) { if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas,
_airspeed.indicated_airspeed_m_s, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)));
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* limit roll motion to ensure enough lift */ /* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
} else { } else {
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_airspeed.indicated_airspeed_m_s, eas2tas, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), false, math::radians(_parameters.pitch_limit_min));
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} }
} else { } else {
@ -1083,12 +1085,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll(); _att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing(); _att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
seatbelt_airspeed, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_airspeed.indicated_airspeed_m_s, eas2tas, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, _parameters.pitch_limit_min, false, math::radians(_parameters.pitch_limit_min));
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* seatbelt mode enabled */) { } else if (0/* seatbelt mode enabled */) {
@ -1127,12 +1127,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _manual.roll; _att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw; _att_sp.yaw_body = _manual.yaw;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
seatbelt_airspeed, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_airspeed.indicated_airspeed_m_s, eas2tas, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, _parameters.pitch_limit_min, climb_out, math::radians(_parameters.pitch_limit_min));
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else { } else {
@ -1328,6 +1326,18 @@ void FixedwingPositionControl::reset_landing_state()
land_onslope = false; land_onslope = false;
} }
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad)
{
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
}
int int
FixedwingPositionControl::start() FixedwingPositionControl::start()
{ {