Skip to content

Commit b4328e7

Browse files
Thomas StastnyJaeyoung-Lim
Thomas Stastny
authored and
Jaeyoung-Lim
committed
eas2tas conversions for npfg input/output.. also some missing get airspeed refs
1 parent 5857461 commit b4328e7

File tree

1 file changed

+26
-22
lines changed

1 file changed

+26
-22
lines changed

‎src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp‎

Lines changed: 26 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -957,12 +957,12 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
957957
float target_airspeed = calculate_target_airspeed(mission_airspeed, ground_speed);
958958

959959
if (_param_fw_use_npfg.get()) {
960-
_npfg.setAirspeedNom(target_airspeed);
961-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
960+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
961+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
962962
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
963963
_att_sp.roll_body = _npfg.getRollSetpoint();
964964
_att_sp.yaw_body = _npfg.getBearing();
965-
target_airspeed = _npfg.getAirspeedRef();
965+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
966966

967967
} else {
968968
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
@@ -1009,12 +1009,12 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
10091009
float target_airspeed = calculate_target_airspeed(mission_airspeed, ground_speed);
10101010

10111011
if (_param_fw_use_npfg.get()) {
1012-
_npfg.setAirspeedNom(target_airspeed);
1013-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1012+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
1013+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
10141014
_npfg.navigateLoiter(curr_wp, curr_pos, loiter_radius, loiter_direction, ground_speed, _wind_vel);
10151015
_att_sp.roll_body = _npfg.getRollSetpoint();
10161016
_att_sp.yaw_body = _npfg.getBearing();
1017-
target_airspeed = _npfg.getAirspeedRef();
1017+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
10181018

10191019
} else {
10201020
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d);
@@ -1167,13 +1167,15 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
11671167

11681168
if (_param_fw_use_npfg.get()) {
11691169
_npfg.enableWindExcessRegulation(false); // disable as we are not letting npfg control airspeed anyway
1170-
_npfg.setAirspeedNom(altctrl_airspeed);
1171-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1170+
_npfg.setAirspeedNom(altctrl_airspeed * _eas2tas);
1171+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
11721172
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
11731173
_att_sp.roll_body = _npfg.getRollSetpoint();
11741174
_att_sp.yaw_body = _npfg.getBearing();
11751175
_npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get()); // reset to user defined value in case we switch modes
11761176

1177+
altctrl_airspeed = _npfg.getAirspeedRef() / _eas2tas;
1178+
11771179
} else {
11781180
/* populate l1 control setpoint */
11791181
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
@@ -1375,12 +1377,14 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
13751377
ground_speed);
13761378

13771379
if (_param_fw_use_npfg.get()) {
1378-
_npfg.setAirspeedNom(target_airspeed);
1379-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1380+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
1381+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
13801382
_npfg.navigateWaypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed, _wind_vel);
13811383
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
13821384
_att_sp.yaw_body = _runway_takeoff.getYaw(_npfg.getBearing());
13831385

1386+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
1387+
13841388
} else {
13851389
/*
13861390
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1463,12 +1467,12 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
14631467
float target_airspeed = _param_fw_airspd_trim.get();
14641468

14651469
if (_param_fw_use_npfg.get()) {
1466-
_npfg.setAirspeedNom(target_airspeed);
1467-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1470+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
1471+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
14681472
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
14691473
_att_sp.roll_body = _npfg.getRollSetpoint();
14701474
_att_sp.yaw_body = _npfg.getBearing();
1471-
target_airspeed = _npfg.getAirspeedRef();
1475+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
14721476

14731477
} else {
14741478
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
@@ -1495,12 +1499,12 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d
14951499
float target_airspeed = calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed);
14961500

14971501
if (_param_fw_use_npfg.get()) {
1498-
_npfg.setAirspeedNom(target_airspeed);
1499-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1502+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
1503+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
15001504
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
15011505
_att_sp.roll_body = _npfg.getRollSetpoint();
15021506
_att_sp.yaw_body = _npfg.getBearing();
1503-
target_airspeed = _npfg.getAirspeedRef();
1507+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
15041508

15051509
} else {
15061510
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
@@ -1676,8 +1680,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
16761680
float target_airspeed = calculate_target_airspeed(airspeed_land, ground_speed);
16771681

16781682
if (_param_fw_use_npfg.get()) {
1679-
_npfg.setAirspeedNom(target_airspeed);
1680-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1683+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
1684+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
16811685

16821686
if (_land_noreturn_horizontal) {
16831687
// heading hold
@@ -1690,7 +1694,7 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
16901694

16911695
_att_sp.roll_body = _npfg.getRollSetpoint();
16921696
_att_sp.yaw_body = _npfg.getBearing();
1693-
target_airspeed = _npfg.getAirspeedRef();
1697+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
16941698

16951699
} else {
16961700
if (_land_noreturn_horizontal) {
@@ -1816,8 +1820,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
18161820
float target_airspeed = calculate_target_airspeed(airspeed_approach, ground_speed);
18171821

18181822
if (_param_fw_use_npfg.get()) {
1819-
_npfg.setAirspeedNom(target_airspeed);
1820-
_npfg.setAirspeedMax(_param_fw_airspd_max.get());
1823+
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
1824+
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
18211825

18221826
if (_land_noreturn_horizontal) {
18231827
// heading hold
@@ -1830,7 +1834,7 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d
18301834

18311835
_att_sp.roll_body = _npfg.getRollSetpoint();
18321836
_att_sp.yaw_body = _npfg.getBearing();
1833-
target_airspeed = _npfg.getAirspeedRef();
1837+
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
18341838

18351839
} else {
18361840
if (_land_noreturn_horizontal) {

0 commit comments

Comments
 (0)