2014-07-24 03:21:30 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2015-05-13 03:09:36 -03:00
# include "Plane.h"
2014-07-24 03:21:30 -03:00
/*
2014-07-24 04:54:58 -03:00
altitude handling routines . These cope with both barometric control
and terrain following control
2014-07-24 03:21:30 -03:00
*/
/*
adjust altitude target depending on mode
*/
2015-05-13 03:09:36 -03:00
void Plane : : adjust_altitude_target ( )
2014-07-24 03:21:30 -03:00
{
2017-02-14 15:13:11 -04:00
Location target_location ;
2019-01-15 13:46:13 -04:00
if ( control_mode = = & mode_fbwb | |
control_mode = = & mode_cruise ) {
2014-07-24 03:21:30 -03:00
return ;
}
2020-05-16 01:02:01 -03:00
# if OFFBOARD_GUIDED == ENABLED
if ( control_mode = = & mode_guided & & ( ( guided_state . target_alt_time_ms ! = 0 ) | | guided_state . target_alt > - 0.001 ) ) { // target_alt now defaults to -1, and _time_ms defaults to zero.
// offboard altitude demanded
uint32_t now = AP_HAL : : millis ( ) ;
float delta = 1e-3 f * ( now - guided_state . target_alt_time_ms ) ;
guided_state . target_alt_time_ms = now ;
// determine delta accurately as a float
float delta_amt_f = delta * guided_state . target_alt_accel ;
// then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
int32_t delta_amt_i = ( int32_t ) ( 100.0 * delta_amt_f ) ;
Location temp { } ;
temp . alt = guided_state . last_target_alt + delta_amt_i ; // ...to avoid floats here,
if ( is_positive ( guided_state . target_alt_accel ) ) {
temp . alt = MIN ( guided_state . target_alt , temp . alt ) ;
} else {
temp . alt = MAX ( guided_state . target_alt , temp . alt ) ;
}
guided_state . last_target_alt = temp . alt ;
set_target_altitude_location ( temp ) ;
} else
# endif // OFFBOARD_GUIDED == ENABLED
2021-06-01 03:03:52 -03:00
if ( control_mode - > update_target_altitude ( ) ) {
// handled in mode specific code
} else if ( landing . is_flaring ( ) ) {
2017-01-10 03:42:57 -04:00
// during a landing flare, use TECS_LAND_SINK as a target sink
2014-08-11 03:46:08 -03:00
// rate, and ignores the target altitude
set_target_altitude_location ( next_WP_loc ) ;
2017-01-10 03:42:57 -04:00
} else if ( landing . is_on_approach ( ) ) {
2016-11-21 20:09:32 -04:00
landing . setup_landing_glide_slope ( prev_WP_loc , next_WP_loc , current_loc , target_altitude . offset_cm ) ;
landing . adjust_landing_slope_for_rangefinder_bump ( rangefinder_state , prev_WP_loc , next_WP_loc , current_loc , auto_state . wp_distance , target_altitude . offset_cm ) ;
2017-02-14 15:13:11 -04:00
} else if ( landing . get_target_altitude_location ( target_location ) ) {
set_target_altitude_location ( target_location ) ;
2020-09-23 05:16:45 -03:00
# if HAL_SOARING_ENABLED
2020-04-05 08:38:19 -03:00
} else if ( g2 . soaring_controller . is_active ( ) & & g2 . soaring_controller . get_throttle_suppressed ( ) ) {
2019-07-15 08:23:46 -03:00
// Reset target alt to current alt, to prevent large altitude errors when gliding.
set_target_altitude_location ( current_loc ) ;
reset_offset_altitude ( ) ;
2019-08-22 21:17:14 -03:00
# endif
2016-05-07 21:40:42 -03:00
} else if ( reached_loiter_target ( ) ) {
2014-07-24 03:21:30 -03:00
// once we reach a loiter target then lock to the final
// altitude target
set_target_altitude_location ( next_WP_loc ) ;
2014-08-13 20:49:52 -03:00
} else if ( target_altitude . offset_cm ! = 0 & &
2019-04-12 05:23:04 -03:00
! current_loc . past_interval_finish_line ( prev_WP_loc , next_WP_loc ) ) {
2014-07-24 03:21:30 -03:00
// control climb/descent rate
2015-01-01 00:17:45 -04:00
set_target_altitude_proportion ( next_WP_loc , 1.0f - auto_state . wp_proportion ) ;
2014-07-24 03:21:30 -03:00
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location ( next_WP_loc , prev_WP_loc ) ;
2016-04-26 08:27:12 -03:00
} else {
2014-07-24 03:21:30 -03:00
set_target_altitude_location ( next_WP_loc ) ;
}
altitude_error_cm = calc_altitude_error_cm ( ) ;
}
/*
setup for a gradual glide slope to the next waypoint , if appropriate
*/
2015-05-13 03:09:36 -03:00
void Plane : : setup_glide_slope ( void )
2014-07-24 03:21:30 -03:00
{
// establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude
2019-02-24 20:11:45 -04:00
auto_state . wp_distance = current_loc . get_distance ( next_WP_loc ) ;
2019-04-12 05:23:04 -03:00
auto_state . wp_proportion = current_loc . line_path_proportion ( prev_WP_loc , next_WP_loc ) ;
2021-11-12 13:53:28 -04:00
TECS_controller . set_path_proportion ( auto_state . wp_proportion ) ;
2016-04-19 19:41:02 -03:00
update_flight_stage ( ) ;
2014-07-24 03:21:30 -03:00
/*
work out if we will gradually change altitude , or try to get to
the new altitude as quickly as possible .
*/
2019-01-15 13:46:13 -04:00
switch ( control_mode - > mode_number ( ) ) {
case Mode : : Number : : RTL :
case Mode : : Number : : AVOID_ADSB :
case Mode : : Number : : GUIDED :
2014-07-24 03:21:30 -03:00
/* glide down slowly if above target altitude, but ascend more
rapidly if below it . See
2016-03-25 06:47:03 -03:00
https : //github.com/ArduPilot/ardupilot/issues/39
2014-07-24 03:21:30 -03:00
*/
2014-07-24 04:54:58 -03:00
if ( above_location_current ( next_WP_loc ) ) {
2021-11-10 14:42:59 -04:00
set_offset_altitude_location ( prev_WP_loc , next_WP_loc ) ;
2014-07-24 03:21:30 -03:00
} else {
reset_offset_altitude ( ) ;
}
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : AUTO :
2014-08-04 06:59:33 -03:00
// we only do glide slide handling in AUTO when above 20m or
// when descending. The 20 meter threshold is arbitrary, and
2014-07-24 03:21:30 -03:00
// is basically to prevent situations where we try to slowly
// gain height at low altitudes, potentially hitting
// obstacles.
2015-04-13 02:17:41 -03:00
if ( adjusted_relative_altitude_cm ( ) > 2000 | | above_location_current ( next_WP_loc ) ) {
2021-11-10 14:42:59 -04:00
set_offset_altitude_location ( prev_WP_loc , next_WP_loc ) ;
2014-07-24 03:21:30 -03:00
} else {
reset_offset_altitude ( ) ;
}
break ;
default :
reset_offset_altitude ( ) ;
break ;
}
}
/*
2021-06-01 03:03:52 -03:00
return RTL altitude as AMSL cm
2014-07-24 03:21:30 -03:00
*/
2021-06-01 03:03:52 -03:00
int32_t Plane : : get_RTL_altitude_cm ( ) const
2014-07-24 03:21:30 -03:00
{
if ( g . RTL_altitude_cm < 0 ) {
return current_loc . alt ;
}
return g . RTL_altitude_cm + home . alt ;
}
2016-05-05 08:01:28 -03:00
/*
return relative altitude in meters ( relative to terrain , if available ,
or home otherwise )
*/
2016-06-03 18:10:26 -03:00
float Plane : : relative_ground_altitude ( bool use_rangefinder_if_available )
2016-05-05 08:01:28 -03:00
{
2016-06-16 18:19:10 -03:00
if ( use_rangefinder_if_available & & rangefinder_state . in_range ) {
2016-06-03 18:10:26 -03:00
return rangefinder_state . height_estimate ;
2019-09-11 04:20:04 -03:00
}
2021-09-10 03:28:21 -03:00
# if HAL_QUADPLANE_ENABLED
2019-09-11 04:20:04 -03:00
if ( use_rangefinder_if_available & & quadplane . in_vtol_land_final ( ) & &
2019-11-01 02:14:01 -03:00
rangefinder . status_orient ( ROTATION_PITCH_270 ) = = RangeFinder : : Status : : OutOfRangeLow ) {
2019-09-11 04:20:04 -03:00
// a special case for quadplane landing when rangefinder goes
// below minimum. Consider our height above ground to be zero
return 0 ;
}
2021-09-10 03:28:21 -03:00
# endif
2016-06-03 18:10:26 -03:00
2016-05-05 08:01:28 -03:00
# if AP_TERRAIN_AVAILABLE
float altitude ;
2016-06-10 04:47:32 -03:00
if ( target_altitude . terrain_following & &
terrain . status ( ) = = AP_Terrain : : TerrainStatusOK & &
terrain . height_above_terrain ( altitude , true ) ) {
2016-05-05 08:01:28 -03:00
return altitude ;
}
# endif
2019-02-23 22:53:52 -04:00
2021-09-10 03:28:21 -03:00
# if HAL_QUADPLANE_ENABLED
2019-02-23 22:53:52 -04:00
if ( quadplane . in_vtol_land_descent ( ) & &
! ( quadplane . options & QuadPlane : : OPTION_MISSION_LAND_FW_APPROACH ) ) {
// when doing a VTOL landing we can use the waypoint height as
// ground height. We can't do this if using the
// LAND_FW_APPROACH as that uses the wp height as the approach
// height
return height_above_target ( ) ;
}
2021-09-10 03:28:21 -03:00
# endif
2019-02-23 22:53:52 -04:00
2017-01-30 15:48:22 -04:00
return relative_altitude ;
2016-05-05 08:01:28 -03:00
}
2014-07-24 03:21:30 -03:00
/*
set the target altitude to the current altitude . This is used when
setting up for altitude hold , such as when releasing elevator in
CRUISE mode .
*/
2015-05-13 03:09:36 -03:00
void Plane : : set_target_altitude_current ( void )
2014-07-24 03:21:30 -03:00
{
2014-07-24 04:54:58 -03:00
// record altitude above sea level at the current time as our
// target altitude
2014-07-24 03:21:30 -03:00
target_altitude . amsl_cm = current_loc . alt ;
2014-07-24 04:54:58 -03:00
// reset any glide slope offset
reset_offset_altitude ( ) ;
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 04:54:58 -03:00
// also record the terrain altitude if possible
float terrain_altitude ;
2021-03-19 20:16:25 -03:00
if ( terrain_enabled_in_current_mode ( ) & & terrain . height_above_terrain ( terrain_altitude , true ) & & ! terrain_disabled ( ) ) {
2014-07-24 04:54:58 -03:00
target_altitude . terrain_following = true ;
target_altitude . terrain_alt_cm = terrain_altitude * 100 ;
} else {
// if terrain following is disabled, or we don't know our
// terrain altitude when we set the altitude then don't
// terrain follow
target_altitude . terrain_following = false ;
}
# endif
2014-07-24 03:21:30 -03:00
}
/*
set the target altitude to the current altitude , with ALT_OFFSET adjustment
*/
2015-05-13 03:09:36 -03:00
void Plane : : set_target_altitude_current_adjusted ( void )
2014-07-24 03:21:30 -03:00
{
2014-07-24 04:54:58 -03:00
set_target_altitude_current ( ) ;
// use adjusted_altitude_cm() to take account of ALTITUDE_OFFSET
2014-07-24 03:21:30 -03:00
target_altitude . amsl_cm = adjusted_altitude_cm ( ) ;
}
/*
set target altitude based on a location structure
*/
2015-05-13 03:09:36 -03:00
void Plane : : set_target_altitude_location ( const Location & loc )
2014-07-24 03:21:30 -03:00
{
target_altitude . amsl_cm = loc . alt ;
2019-01-02 00:46:09 -04:00
if ( loc . relative_alt ) {
2014-07-24 04:54:58 -03:00
target_altitude . amsl_cm + = home . alt ;
}
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 04:54:58 -03:00
/*
if this location has the terrain_alt flag set and we know the
terrain altitude of our current location then treat it as a
terrain altitude
*/
float height ;
2019-01-02 00:46:09 -04:00
if ( loc . terrain_alt & & terrain . height_above_terrain ( height , true ) ) {
2014-07-24 04:54:58 -03:00
target_altitude . terrain_following = true ;
target_altitude . terrain_alt_cm = loc . alt ;
2019-01-02 00:46:09 -04:00
if ( ! loc . relative_alt ) {
2014-07-24 04:54:58 -03:00
// it has home added, remove it
target_altitude . terrain_alt_cm - = home . alt ;
}
} else {
target_altitude . terrain_following = false ;
}
# endif
2014-07-24 03:21:30 -03:00
}
/*
return relative to home target altitude in centimeters . Used for
altitude control libraries
*/
2015-05-13 03:09:36 -03:00
int32_t Plane : : relative_target_altitude_cm ( void )
2014-07-24 03:21:30 -03:00
{
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 04:54:58 -03:00
float relative_home_height ;
if ( target_altitude . terrain_following & &
2014-08-05 22:44:06 -03:00
terrain . height_relative_home_equivalent ( target_altitude . terrain_alt_cm * 0.01f ,
relative_home_height , true ) ) {
2014-08-06 20:31:03 -03:00
// add lookahead adjustment the target altitude
target_altitude . lookahead = lookahead_adjustment ( ) ;
relative_home_height + = target_altitude . lookahead ;
2014-08-26 08:18:24 -03:00
// correct for rangefinder data
relative_home_height + = rangefinder_correction ( ) ;
2014-07-24 04:54:58 -03:00
// we are following terrain, and have terrain data for the
// current location. Use it.
return relative_home_height * 100 ;
}
# endif
2014-08-26 08:18:24 -03:00
int32_t relative_alt = target_altitude . amsl_cm - home . alt ;
2016-07-19 20:09:21 -03:00
relative_alt + = mission_alt_offset ( ) * 100 ;
2014-08-26 08:18:24 -03:00
relative_alt + = rangefinder_correction ( ) * 100 ;
return relative_alt ;
2014-07-24 03:21:30 -03:00
}
/*
change the current target altitude by an amount in centimeters . Used
to cope with changes due to elevator in CRUISE or FBWB
*/
2015-05-13 03:09:36 -03:00
void Plane : : change_target_altitude ( int32_t change_cm )
2014-07-24 03:21:30 -03:00
{
target_altitude . amsl_cm + = change_cm ;
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2020-07-07 09:25:21 -03:00
if ( target_altitude . terrain_following & & ! terrain_disabled ( ) ) {
2014-07-24 04:54:58 -03:00
target_altitude . terrain_alt_cm + = change_cm ;
}
# endif
2014-07-24 03:21:30 -03:00
}
/*
change target altitude by a proportion of the target altitude offset
( difference in height to next WP from previous WP ) . proportion
should be between 0 and 1.
When proportion is zero we have reached the destination . When
proportion is 1 we are at the starting waypoint .
Note that target_altitude is setup initially based on the
destination waypoint
*/
2015-05-13 03:09:36 -03:00
void Plane : : set_target_altitude_proportion ( const Location & loc , float proportion )
2014-07-24 03:21:30 -03:00
{
set_target_altitude_location ( loc ) ;
proportion = constrain_float ( proportion , 0.0f , 1.0f ) ;
change_target_altitude ( - target_altitude . offset_cm * proportion ) ;
2015-04-12 23:20:01 -03:00
//rebuild the glide slope if we are above it and supposed to be climbing
if ( g . glide_slope_threshold > 0 ) {
if ( target_altitude . offset_cm > 0 & & calc_altitude_error_cm ( ) < - 100 * g . glide_slope_threshold ) {
set_target_altitude_location ( loc ) ;
2021-11-10 14:42:59 -04:00
set_offset_altitude_location ( current_loc , loc ) ;
2015-04-12 23:20:01 -03:00
change_target_altitude ( - target_altitude . offset_cm * proportion ) ;
//adjust the new target offset altitude to reflect that we are partially already done
if ( proportion > 0.0f )
target_altitude . offset_cm = ( ( float ) target_altitude . offset_cm ) / proportion ;
}
}
2014-07-24 03:21:30 -03:00
}
/*
constrain target altitude to be between two locations . Used to
ensure we stay within two waypoints in altitude
*/
2015-05-13 03:09:36 -03:00
void Plane : : constrain_target_altitude_location ( const Location & loc1 , const Location & loc2 )
2014-07-24 03:21:30 -03:00
{
2014-07-24 06:56:15 -03:00
if ( loc1 . alt > loc2 . alt ) {
2014-07-24 03:21:30 -03:00
target_altitude . amsl_cm = constrain_int32 ( target_altitude . amsl_cm , loc2 . alt , loc1 . alt ) ;
} else {
target_altitude . amsl_cm = constrain_int32 ( target_altitude . amsl_cm , loc1 . alt , loc2 . alt ) ;
}
}
/*
return error between target altitude and current altitude
*/
2015-05-13 03:09:36 -03:00
int32_t Plane : : calc_altitude_error_cm ( void )
2014-07-24 03:21:30 -03:00
{
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 04:54:58 -03:00
float terrain_height ;
if ( target_altitude . terrain_following & &
2014-08-05 22:44:06 -03:00
terrain . height_above_terrain ( terrain_height , true ) ) {
2014-08-06 20:31:03 -03:00
return target_altitude . lookahead * 100 + target_altitude . terrain_alt_cm - ( terrain_height * 100 ) ;
2014-07-24 04:54:58 -03:00
}
# endif
2014-07-24 03:21:30 -03:00
return target_altitude . amsl_cm - adjusted_altitude_cm ( ) ;
}
/*
check for FBWB_min_altitude_cm violation
*/
2017-11-23 00:56:08 -04:00
void Plane : : check_fbwb_minimum_altitude ( void )
2014-07-24 03:21:30 -03:00
{
2014-07-24 04:54:58 -03:00
if ( g . FBWB_min_altitude_cm = = 0 ) {
return ;
}
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 04:54:58 -03:00
if ( target_altitude . terrain_following ) {
// set our target terrain height to be at least the min set
if ( target_altitude . terrain_alt_cm < g . FBWB_min_altitude_cm ) {
target_altitude . terrain_alt_cm = g . FBWB_min_altitude_cm ;
}
return ;
}
# endif
if ( target_altitude . amsl_cm < home . alt + g . FBWB_min_altitude_cm ) {
2014-07-24 03:21:30 -03:00
target_altitude . amsl_cm = home . alt + g . FBWB_min_altitude_cm ;
}
}
/*
reset the altitude offset used for glide slopes
*/
2015-05-13 03:09:36 -03:00
void Plane : : reset_offset_altitude ( void )
2014-07-24 03:21:30 -03:00
{
target_altitude . offset_cm = 0 ;
}
2014-08-27 01:33:57 -03:00
2014-07-24 03:21:30 -03:00
/*
reset the altitude offset used for glide slopes , based on difference
2021-11-10 14:42:59 -04:00
between altitude at a destination and a specified start altitude . If
destination is above the starting altitude then the result is
2014-07-24 03:21:30 -03:00
positive .
*/
2021-11-10 14:42:59 -04:00
void Plane : : set_offset_altitude_location ( const Location & start_loc , const Location & destination_loc )
2014-07-24 03:21:30 -03:00
{
2021-11-10 14:42:59 -04:00
target_altitude . offset_cm = destination_loc . alt - start_loc . alt ;
2014-07-24 04:54:58 -03:00
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 20:07:45 -03:00
/*
if this location has the terrain_alt flag set and we know the
terrain altitude of our current location then treat it as a
terrain altitude
*/
float height ;
2021-11-10 14:42:59 -04:00
if ( destination_loc . terrain_alt & &
2014-07-24 20:07:45 -03:00
target_altitude . terrain_following & &
2014-08-05 22:44:06 -03:00
terrain . height_above_terrain ( height , true ) ) {
2014-07-24 20:07:45 -03:00
target_altitude . offset_cm = target_altitude . terrain_alt_cm - ( height * 100 ) ;
2014-07-24 04:54:58 -03:00
}
# endif
2014-08-04 04:13:30 -03:00
2017-01-11 01:29:03 -04:00
if ( flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_LAND ) {
2014-08-04 06:58:12 -03:00
// if we are within GLIDE_SLOPE_MIN meters of the target altitude
// then reset the offset to not use a glide slope. This allows for
// more accurate flight of missions where the aircraft may lose or
// gain a bit of altitude near waypoint turn points due to local
// terrain changes
2015-04-12 23:20:01 -03:00
if ( g . glide_slope_min < = 0 | |
labs ( target_altitude . offset_cm ) * 0.01f < g . glide_slope_min ) {
2014-08-04 06:58:12 -03:00
target_altitude . offset_cm = 0 ;
}
2014-08-04 04:13:30 -03:00
}
2014-07-24 04:54:58 -03:00
}
/*
2014-07-24 06:56:15 -03:00
return true if current_loc is above loc . Used for glide slope
calculations .
" above " is simple if we are not terrain following , as it just means
the pressure altitude of one is above the other .
When in terrain following mode " above " means the over - the - terrain
current altitude is above the over - the - terrain alt of loc . It is
quite possible for current_loc to be " above " loc when it is at a
lower pressure altitude , if current_loc is in a low part of the
terrain
2014-07-24 04:54:58 -03:00
*/
2015-05-13 03:09:36 -03:00
bool Plane : : above_location_current ( const Location & loc )
2014-07-24 04:54:58 -03:00
{
2014-07-24 18:58:39 -03:00
# if AP_TERRAIN_AVAILABLE
2014-07-24 06:56:15 -03:00
float terrain_alt ;
2019-01-02 00:46:09 -04:00
if ( loc . terrain_alt & &
2014-08-05 22:44:06 -03:00
terrain . height_above_terrain ( terrain_alt , true ) ) {
2014-07-24 06:56:15 -03:00
float loc_alt = loc . alt * 0.01f ;
2019-01-02 00:46:09 -04:00
if ( ! loc . relative_alt ) {
2014-07-24 06:56:15 -03:00
loc_alt - = home . alt * 0.01f ;
}
2014-08-04 03:55:15 -03:00
return terrain_alt > loc_alt ;
2014-07-24 04:54:58 -03:00
}
# endif
2014-07-24 03:21:30 -03:00
2014-08-04 03:55:15 -03:00
float loc_alt_cm = loc . alt ;
2019-01-02 00:46:09 -04:00
if ( loc . relative_alt ) {
2015-07-28 19:20:23 -03:00
loc_alt_cm + = home . alt ;
2014-08-04 03:55:15 -03:00
}
return current_loc . alt > loc_alt_cm ;
2014-07-24 04:54:58 -03:00
}
/*
modify a destination to be setup for terrain following if
TERRAIN_FOLLOW is enabled
*/
2021-02-01 12:26:22 -04:00
void Plane : : setup_terrain_target_alt ( Location & loc ) const
2014-07-24 04:54:58 -03:00
{
2014-07-24 19:02:19 -03:00
# if AP_TERRAIN_AVAILABLE
2021-03-19 20:16:25 -03:00
if ( terrain_enabled_in_current_mode ( ) ) {
2021-04-07 14:41:53 -03:00
loc . change_alt_frame ( Location : : AltFrame : : ABOVE_TERRAIN ) ;
2014-07-24 04:54:58 -03:00
}
2014-07-24 19:02:19 -03:00
# endif
2014-07-24 04:54:58 -03:00
}
/*
return current_loc . alt adjusted for ALT_OFFSET
This is useful during long flights to account for barometer changes
from the GCS , or to adjust the flying height of a long mission
*/
2015-05-13 03:09:36 -03:00
int32_t Plane : : adjusted_altitude_cm ( void )
2014-07-24 03:21:30 -03:00
{
2016-07-19 20:09:21 -03:00
return current_loc . alt - ( mission_alt_offset ( ) * 100 ) ;
2014-07-24 03:21:30 -03:00
}
2014-08-06 20:31:03 -03:00
2015-02-25 07:54:52 -04:00
/*
return home - relative altitude adjusted for ALT_OFFSET This is useful
during long flights to account for barometer changes from the GCS ,
or to adjust the flying height of a long mission
*/
2015-05-13 03:09:36 -03:00
int32_t Plane : : adjusted_relative_altitude_cm ( void )
2015-02-25 07:54:52 -04:00
{
2017-01-30 15:48:22 -04:00
return ( relative_altitude - mission_alt_offset ( ) ) * 100 ;
2015-02-25 07:54:52 -04:00
}
2016-07-19 20:09:21 -03:00
/*
return the mission altitude offset . This raises or lowers all
mission items . It is primarily set using the ALT_OFFSET parameter ,
but can also be adjusted by the rangefinder landing code for a
NAV_LAND command if we have aborted a steep landing
*/
float Plane : : mission_alt_offset ( void )
{
float ret = g . alt_offset ;
2019-01-15 13:46:13 -04:00
if ( control_mode = = & mode_auto & &
2017-01-11 01:29:03 -04:00
( flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND | | auto_state . wp_is_land_approach ) ) {
2016-07-19 20:09:21 -03:00
// when landing after an aborted landing due to too high glide
// slope we use an offset from the last landing attempt
2016-11-16 21:12:26 -04:00
ret + = landing . alt_offset ;
2016-07-19 20:09:21 -03:00
}
return ret ;
}
2014-08-11 03:46:08 -03:00
/*
return the height in meters above the next_WP_loc altitude
*/
2015-05-13 03:09:36 -03:00
float Plane : : height_above_target ( void )
2014-08-11 03:46:08 -03:00
{
float target_alt = next_WP_loc . alt * 0.01 ;
2019-01-02 00:46:09 -04:00
if ( ! next_WP_loc . relative_alt ) {
2015-04-11 06:43:13 -03:00
target_alt - = ahrs . get_home ( ) . alt * 0.01f ;
2014-08-11 03:46:08 -03:00
}
# if AP_TERRAIN_AVAILABLE
// also record the terrain altitude if possible
float terrain_altitude ;
2019-01-02 00:46:09 -04:00
if ( next_WP_loc . terrain_alt & &
2014-08-11 03:46:08 -03:00
terrain . height_above_terrain ( terrain_altitude , true ) ) {
return terrain_altitude - target_alt ;
}
# endif
2014-08-13 19:04:06 -03:00
return ( adjusted_altitude_cm ( ) * 0.01f - ahrs . get_home ( ) . alt * 0.01f ) - target_alt ;
2014-08-11 03:46:08 -03:00
}
2014-08-06 20:31:03 -03:00
/*
work out target altitude adjustment from terrain lookahead
*/
2015-05-13 03:09:36 -03:00
float Plane : : lookahead_adjustment ( void )
2014-08-06 20:31:03 -03:00
{
# if AP_TERRAIN_AVAILABLE
int32_t bearing_cd ;
int16_t distance ;
// work out distance and bearing to target
2019-01-15 13:46:13 -04:00
if ( control_mode = = & mode_fbwb ) {
2014-08-06 20:31:03 -03:00
// there is no target waypoint in FBWB, so use yaw as an approximation
bearing_cd = ahrs . yaw_sensor ;
distance = g . terrain_lookahead ;
2016-05-07 21:40:42 -03:00
} else if ( ! reached_loiter_target ( ) ) {
2014-08-06 20:31:03 -03:00
bearing_cd = nav_controller - > target_bearing_cd ( ) ;
2015-01-01 00:17:45 -04:00
distance = constrain_float ( auto_state . wp_distance , 0 , g . terrain_lookahead ) ;
2014-08-06 20:31:03 -03:00
} else {
// no lookahead when loitering
bearing_cd = 0 ;
distance = 0 ;
}
if ( distance < = 0 ) {
// no lookahead
return 0 ;
}
float groundspeed = ahrs . groundspeed ( ) ;
if ( groundspeed < 1 ) {
// we're not moving
return 0 ;
}
// we need to know the climb ratio. We use 50% of the maximum
// climb rate so we are not constantly at 100% throttle and to
// give a bit more margin on terrain
2021-11-12 13:53:28 -04:00
float climb_ratio = 0.5f * TECS_controller . get_max_climbrate ( ) / groundspeed ;
2014-08-06 20:31:03 -03:00
if ( climb_ratio < = 0 ) {
// lookahead makes no sense for negative climb rates
return 0 ;
}
// ask the terrain code for the lookahead altitude change
float lookahead = terrain . lookahead ( bearing_cd * 0.01f , distance , climb_ratio ) ;
if ( target_altitude . offset_cm < 0 ) {
// we are heading down to the waypoint, so we don't need to
// climb as much
lookahead + = target_altitude . offset_cm * 0.01f ;
}
// constrain lookahead to a reasonable limit
return constrain_float ( lookahead , 0 , 1000.0f ) ;
2014-08-08 00:54:05 -03:00
# else
return 0 ;
2014-08-06 20:31:03 -03:00
# endif
}
2014-08-26 08:18:24 -03:00
/*
correct target altitude using rangefinder data . Returns offset in
meters to correct target altitude . A positive number means we need
to ask the speed / height controller to fly higher
*/
2015-05-13 03:09:36 -03:00
float Plane : : rangefinder_correction ( void )
2014-08-26 08:18:24 -03:00
{
2015-05-13 19:05:32 -03:00
if ( millis ( ) - rangefinder_state . last_correction_time_ms > 5000 ) {
2014-08-26 22:50:07 -03:00
// we haven't had any rangefinder data for 5s - don't use it
2014-08-26 08:18:24 -03:00
return 0 ;
}
// for now we only support the rangefinder for landing
2017-01-11 01:29:03 -04:00
bool using_rangefinder = ( g . rangefinder_landing & & flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND ) ;
2014-08-26 08:18:24 -03:00
if ( ! using_rangefinder ) {
2014-08-26 22:50:07 -03:00
return 0 ;
2014-08-26 08:18:24 -03:00
}
2014-08-26 22:50:07 -03:00
return rangefinder_state . correction ;
}
2014-08-26 08:18:24 -03:00
2019-04-06 01:53:16 -03:00
/*
correct rangefinder data for terrain height difference between
NAV_LAND point and current location
*/
void Plane : : rangefinder_terrain_correction ( float & height )
{
# if AP_TERRAIN_AVAILABLE
if ( ! g . rangefinder_landing | |
flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_LAND | |
2021-03-19 20:16:25 -03:00
! terrain_enabled_in_current_mode ( ) ) {
2019-04-06 01:53:16 -03:00
return ;
}
float terrain_amsl1 , terrain_amsl2 ;
if ( ! terrain . height_amsl ( current_loc , terrain_amsl1 , false ) | |
! terrain . height_amsl ( next_WP_loc , terrain_amsl2 , false ) ) {
return ;
}
float correction = ( terrain_amsl1 - terrain_amsl2 ) ;
height + = correction ;
2019-09-14 19:32:12 -03:00
auto_state . terrain_correction = correction ;
2019-04-06 01:53:16 -03:00
# endif
}
2014-08-26 22:50:07 -03:00
/*
update the offset between rangefinder height and terrain height
*/
2015-05-13 03:09:36 -03:00
void Plane : : rangefinder_height_update ( void )
2014-08-26 22:50:07 -03:00
{
2021-10-18 02:45:34 -03:00
float distance = rangefinder . distance_orient ( ROTATION_PITCH_270 ) ;
2016-10-23 01:53:12 -03:00
2019-11-01 02:14:01 -03:00
if ( ( rangefinder . status_orient ( ROTATION_PITCH_270 ) = = RangeFinder : : Status : : Good ) & & ahrs . home_is_set ( ) ) {
2015-09-15 22:14:26 -03:00
if ( ! rangefinder_state . have_initial_reading ) {
rangefinder_state . have_initial_reading = true ;
rangefinder_state . initial_range = distance ;
}
2014-08-26 22:50:07 -03:00
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
2016-03-09 03:20:41 -04:00
rangefinder_state . height_estimate = distance * ahrs . get_rotation_body_to_ned ( ) . c . z ;
2014-08-27 05:25:17 -03:00
2019-04-06 01:53:16 -03:00
rangefinder_terrain_correction ( rangefinder_state . height_estimate ) ;
2014-08-26 22:50:07 -03:00
// we consider ourselves to be fully in range when we have 10
2015-09-15 22:14:26 -03:00
// good samples (0.2s) that are different by 5% of the maximum
// range from the initial range we see. The 5% change is to
// catch Lidars that are giving a constant range, either due
// to misconfiguration or a faulty sensor
2014-08-26 22:50:07 -03:00
if ( rangefinder_state . in_range_count < 10 ) {
2016-10-23 01:53:12 -03:00
if ( ! is_equal ( distance , rangefinder_state . last_distance ) & &
2017-02-09 07:39:58 -04:00
fabsf ( rangefinder_state . initial_range - distance ) > 0.05f * rangefinder . max_distance_cm_orient ( ROTATION_PITCH_270 ) * 0.01f ) {
2015-09-15 22:14:26 -03:00
rangefinder_state . in_range_count + + ;
}
2017-02-09 07:39:58 -04:00
if ( fabsf ( rangefinder_state . last_distance - distance ) > rangefinder . max_distance_cm_orient ( ROTATION_PITCH_270 ) * 0.01 * 0.2 ) {
2016-10-23 01:53:12 -03:00
// changes by more than 20% of full range will reset counter
rangefinder_state . in_range_count = 0 ;
}
2014-08-26 22:50:07 -03:00
} else {
rangefinder_state . in_range = true ;
2021-09-10 03:28:21 -03:00
bool flightstage_good_for_rangefinder_landing = false ;
if ( flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND ) {
flightstage_good_for_rangefinder_landing = true ;
}
# if HAL_QUADPLANE_ENABLED
if ( control_mode = = & mode_qland | |
control_mode = = & mode_qrtl | |
( control_mode = = & mode_auto & & quadplane . is_vtol_land ( plane . mission . get_current_nav_cmd ( ) . id ) ) ) {
flightstage_good_for_rangefinder_landing = true ;
}
# endif
2015-09-15 22:14:26 -03:00
if ( ! rangefinder_state . in_use & &
2021-09-10 03:28:21 -03:00
flightstage_good_for_rangefinder_landing & &
2015-09-15 22:14:26 -03:00
g . rangefinder_landing ) {
rangefinder_state . in_use = true ;
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Rangefinder engaged at %.2fm " , ( double ) rangefinder_state . height_estimate ) ;
2015-09-15 22:14:26 -03:00
}
2014-08-26 22:50:07 -03:00
}
2016-10-23 01:53:12 -03:00
rangefinder_state . last_distance = distance ;
2014-08-26 22:50:07 -03:00
} else {
rangefinder_state . in_range_count = 0 ;
rangefinder_state . in_range = false ;
2014-08-26 08:18:24 -03:00
}
2014-08-26 22:50:07 -03:00
if ( rangefinder_state . in_range ) {
// base correction is the difference between baro altitude and
// rangefinder estimate
2021-07-06 17:34:14 -03:00
float correction = adjusted_relative_altitude_cm ( ) * 0.01 - rangefinder_state . height_estimate ;
2014-08-26 22:50:07 -03:00
# if AP_TERRAIN_AVAILABLE
// if we are terrain following then correction is based on terrain data
float terrain_altitude ;
2021-03-19 20:16:25 -03:00
if ( ( target_altitude . terrain_following | | terrain_enabled_in_current_mode ( ) ) & &
2014-08-26 22:50:07 -03:00
terrain . height_above_terrain ( terrain_altitude , true ) ) {
2016-03-25 20:06:07 -03:00
correction = terrain_altitude - rangefinder_state . height_estimate ;
2014-08-26 22:50:07 -03:00
}
2014-08-26 08:18:24 -03:00
# endif
2014-08-27 05:25:17 -03:00
// remember the last correction. Use a low pass filter unless
// the old data is more than 5 seconds old
2016-05-17 22:21:21 -03:00
uint32_t now = millis ( ) ;
if ( now - rangefinder_state . last_correction_time_ms > 5000 ) {
2014-08-27 05:25:17 -03:00
rangefinder_state . correction = correction ;
2015-09-15 22:14:26 -03:00
rangefinder_state . initial_correction = correction ;
2020-11-02 16:59:29 -04:00
if ( g . rangefinder_landing ) {
landing . set_initial_slope ( ) ;
}
2016-10-17 18:34:00 -03:00
rangefinder_state . last_correction_time_ms = now ;
2014-08-27 05:25:17 -03:00
} else {
rangefinder_state . correction = 0.8f * rangefinder_state . correction + 0.2f * correction ;
2016-10-17 18:34:00 -03:00
rangefinder_state . last_correction_time_ms = now ;
2015-09-15 22:14:26 -03:00
if ( fabsf ( rangefinder_state . correction - rangefinder_state . initial_correction ) > 30 ) {
// the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar
if ( rangefinder_state . in_use ) {
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Rangefinder disengaged at %.2fm " , ( double ) rangefinder_state . height_estimate ) ;
2015-09-15 22:14:26 -03:00
}
memset ( & rangefinder_state , 0 , sizeof ( rangefinder_state ) ) ;
}
2014-08-27 05:25:17 -03:00
}
2016-10-17 18:34:00 -03:00
2014-08-26 22:50:07 -03:00
}
2014-08-26 08:18:24 -03:00
}
2020-07-07 09:25:21 -03:00
/*
determine if Non Auto Terrain Disable is active and allowed in present control mode
*/
bool Plane : : terrain_disabled ( )
{
return control_mode - > allows_terrain_disable ( ) & & non_auto_terrain_disable ;
}
2021-03-19 20:16:25 -03:00
/*
Check if terrain following is enabled for the current mode
*/
# if AP_TERRAIN_AVAILABLE
const Plane : : TerrainLookupTable Plane : : Terrain_lookup [ ] = {
{ Mode : : Number : : FLY_BY_WIRE_B , terrain_bitmask : : FLY_BY_WIRE_B } ,
{ Mode : : Number : : CRUISE , terrain_bitmask : : CRUISE } ,
{ Mode : : Number : : AUTO , terrain_bitmask : : AUTO } ,
{ Mode : : Number : : RTL , terrain_bitmask : : RTL } ,
{ Mode : : Number : : AVOID_ADSB , terrain_bitmask : : AVOID_ADSB } ,
{ Mode : : Number : : GUIDED , terrain_bitmask : : GUIDED } ,
{ Mode : : Number : : LOITER , terrain_bitmask : : LOITER } ,
{ Mode : : Number : : CIRCLE , terrain_bitmask : : CIRCLE } ,
2021-09-10 03:28:21 -03:00
# if HAL_QUADPLANE_ENABLED
2021-03-19 20:16:25 -03:00
{ Mode : : Number : : QRTL , terrain_bitmask : : QRTL } ,
{ Mode : : Number : : QLAND , terrain_bitmask : : QLAND } ,
{ Mode : : Number : : QLOITER , terrain_bitmask : : QLOITER } ,
2021-09-10 03:28:21 -03:00
# endif
2021-03-19 20:16:25 -03:00
} ;
bool Plane : : terrain_enabled_in_current_mode ( ) const
2021-09-18 20:12:33 -03:00
{
return terrain_enabled_in_mode ( control_mode - > mode_number ( ) ) ;
}
bool Plane : : terrain_enabled_in_mode ( Mode : : Number num ) const
2021-03-19 20:16:25 -03:00
{
// Global enable
if ( ( g . terrain_follow . get ( ) & int32_t ( terrain_bitmask : : ALL ) ) ! = 0 ) {
return true ;
}
// Specific enable
for ( const struct TerrainLookupTable entry : Terrain_lookup ) {
2021-09-18 20:12:33 -03:00
if ( entry . mode_num = = num ) {
2021-03-19 20:16:25 -03:00
if ( ( g . terrain_follow . get ( ) & int32_t ( entry . bitmask ) ) ! = 0 ) {
return true ;
}
break ;
}
}
return false ;
}
# endif