mirror of https://github.com/ArduPilot/ardupilot
Plane: added RNGFND_LANDING option
this allows the use of a rangefinder for landing flare and landing approach
This commit is contained in:
parent
b6319a9d19
commit
50f492a69f
|
@ -115,7 +115,7 @@ public:
|
||||||
k_param_takeoff_rotate_speed,
|
k_param_takeoff_rotate_speed,
|
||||||
k_param_takeoff_throttle_slewrate,
|
k_param_takeoff_throttle_slewrate,
|
||||||
k_param_takeoff_throttle_max,
|
k_param_takeoff_throttle_max,
|
||||||
k_param_sonar,
|
k_param_rangefinder,
|
||||||
k_param_terrain,
|
k_param_terrain,
|
||||||
k_param_terrain_follow,
|
k_param_terrain_follow,
|
||||||
k_param_stab_pitch_down_cd_old, // deprecated
|
k_param_stab_pitch_down_cd_old, // deprecated
|
||||||
|
@ -123,6 +123,7 @@ public:
|
||||||
k_param_stab_pitch_down,
|
k_param_stab_pitch_down,
|
||||||
k_param_terrain_lookahead,
|
k_param_terrain_lookahead,
|
||||||
k_param_fbwa_tdrag_chan,
|
k_param_fbwa_tdrag_chan,
|
||||||
|
k_param_rangefinder_landing,
|
||||||
|
|
||||||
// 100: Arming parameters
|
// 100: Arming parameters
|
||||||
k_param_arming = 100,
|
k_param_arming = 100,
|
||||||
|
@ -170,7 +171,7 @@ public:
|
||||||
k_param_curr_amp_per_volt, // unused
|
k_param_curr_amp_per_volt, // unused
|
||||||
k_param_input_voltage, // deprecated, can be deleted
|
k_param_input_voltage, // deprecated, can be deleted
|
||||||
k_param_pack_capacity, // unused
|
k_param_pack_capacity, // unused
|
||||||
k_param_sonar_enabled,
|
k_param_sonar_enabled_old, // unused
|
||||||
k_param_ahrs, // AHRS group
|
k_param_ahrs, // AHRS group
|
||||||
k_param_barometer, // barometer ground calibration
|
k_param_barometer, // barometer ground calibration
|
||||||
k_param_airspeed, // AP_Airspeed parameters
|
k_param_airspeed, // AP_Airspeed parameters
|
||||||
|
@ -459,6 +460,7 @@ public:
|
||||||
#endif
|
#endif
|
||||||
AP_Int16 glide_slope_threshold;
|
AP_Int16 glide_slope_threshold;
|
||||||
AP_Int8 fbwa_tdrag_chan;
|
AP_Int8 fbwa_tdrag_chan;
|
||||||
|
AP_Int8 rangefinder_landing;
|
||||||
|
|
||||||
// RC channels
|
// RC channels
|
||||||
RC_Channel rc_1;
|
RC_Channel rc_1;
|
||||||
|
|
|
@ -916,7 +916,14 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||||
|
|
||||||
// @Group: RNGFND
|
// @Group: RNGFND
|
||||||
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
|
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
|
||||||
GOBJECT(sonar, "RNGFND", RangeFinder),
|
GOBJECT(rangefinder, "RNGFND", RangeFinder),
|
||||||
|
|
||||||
|
// @Param: RNGFND_LANDING
|
||||||
|
// @DisplayName: Enable rangefinder for landing
|
||||||
|
// @Description: This enables the use of a rangefinder for automatic landing. The rangefinder will be used both on the landing approach and for final flare
|
||||||
|
// @Values: 0:Disabled,1:Enabled
|
||||||
|
// @User: Standard
|
||||||
|
GSCALAR(rangefinder_landing, "RNGFND_LANDING", 0),
|
||||||
|
|
||||||
#if AP_TERRAIN_AVAILABLE
|
#if AP_TERRAIN_AVAILABLE
|
||||||
// @Group: TERRAIN_
|
// @Group: TERRAIN_
|
||||||
|
|
|
@ -42,7 +42,7 @@ static void adjust_altitude_target()
|
||||||
location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
|
location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
|
||||||
set_target_altitude_location(loc);
|
set_target_altitude_location(loc);
|
||||||
} else {
|
} else {
|
||||||
set_target_altitude_proportion(next_WP_loc,
|
set_target_altitude_proportion(loc,
|
||||||
(wp_distance-flare_distance) / (wp_totalDistance-flare_distance));
|
(wp_distance-flare_distance) / (wp_totalDistance-flare_distance));
|
||||||
}
|
}
|
||||||
// stay within the range of the start and end locations in altitude
|
// stay within the range of the start and end locations in altitude
|
||||||
|
@ -223,12 +223,19 @@ static int32_t relative_target_altitude_cm(void)
|
||||||
// add lookahead adjustment the target altitude
|
// add lookahead adjustment the target altitude
|
||||||
target_altitude.lookahead = lookahead_adjustment();
|
target_altitude.lookahead = lookahead_adjustment();
|
||||||
relative_home_height += target_altitude.lookahead;
|
relative_home_height += target_altitude.lookahead;
|
||||||
|
|
||||||
|
// correct for rangefinder data
|
||||||
|
relative_home_height += rangefinder_correction();
|
||||||
|
|
||||||
// we are following terrain, and have terrain data for the
|
// we are following terrain, and have terrain data for the
|
||||||
// current location. Use it.
|
// current location. Use it.
|
||||||
return relative_home_height*100;
|
return relative_home_height*100;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
return target_altitude.amsl_cm - home.alt + (int32_t(g.alt_offset)*100);
|
int32_t relative_alt = target_altitude.amsl_cm - home.alt;
|
||||||
|
relative_alt += int32_t(g.alt_offset)*100;
|
||||||
|
relative_alt += rangefinder_correction() * 100;
|
||||||
|
return relative_alt;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -497,3 +504,39 @@ static float lookahead_adjustment(void)
|
||||||
return 0;
|
return 0;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
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
|
||||||
|
*/
|
||||||
|
static float rangefinder_correction(void)
|
||||||
|
{
|
||||||
|
if (!rangefinder_state.in_range) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// for now we only support the rangefinder for landing
|
||||||
|
bool using_rangefinder = (g.rangefinder_landing &&
|
||||||
|
control_mode == AUTO &&
|
||||||
|
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
|
||||||
|
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL));
|
||||||
|
if (!using_rangefinder) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// base correction is the difference between baro altitude and
|
||||||
|
// rangefinder estimate
|
||||||
|
float correction = relative_altitude() - rangefinder_state.height_estimate;
|
||||||
|
|
||||||
|
#if AP_TERRAIN_AVAILABLE
|
||||||
|
// if we are terrain following then correction is based on terrain data
|
||||||
|
float terrain_altitude;
|
||||||
|
if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true)) {
|
||||||
|
correction = terrain_altitude - rangefinder_state.height_estimate;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return correction;
|
||||||
|
}
|
||||||
|
|
|
@ -16,6 +16,9 @@ static bool verify_land()
|
||||||
|
|
||||||
float height = height_above_target();
|
float height = height_above_target();
|
||||||
|
|
||||||
|
// use rangefinder to correct if possible
|
||||||
|
height -= rangefinder_correction();
|
||||||
|
|
||||||
// calculate the sink rate.
|
// calculate the sink rate.
|
||||||
float sink_rate;
|
float sink_rate;
|
||||||
Vector3f vel;
|
Vector3f vel;
|
||||||
|
|
Loading…
Reference in New Issue