Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: resync for 4.0.5

This commit is contained in:
Andrew Tridgell 2020-10-02 10:13:33 +10:00
parent fc306128ba
commit fea5c6ea10
12 changed files with 113 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/AP_Arming.cpp
View File

@ -58,7 +58,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
}
if (plane.channel_throttle->get_reverse() &&
plane.g.throttle_fs_enabled &&
Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled &&
plane.g.throttle_fs_value <
plane.channel_throttle->get_radio_max()) {
check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle");
@ -88,6 +88,21 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
}
}
#if AP_TERRAIN_AVAILABLE
if (plane.g.terrain_follow || plane.mission.contains_terrain_relative()) {
// check terrain data is loaded and healthy
uint16_t terr_pending=0, terr_loaded=0;
plane.terrain.get_statistics(terr_pending, terr_loaded);
if (plane.terrain.status() != AP_Terrain::TerrainStatusOK) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "terrain data unhealthy");
ret = false;
} else if (terr_pending != 0) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "waiting for terrain data");
ret = false;
}
}
#endif
if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
check_failed(display_failure, "No mission loaded");
ret = false;

15
ArduPlane/ArduPlane.cpp
View File

@ -52,7 +52,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(afs_fs_check, 10, 100),
#endif
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500),
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 500),
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150),
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300),
SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150),
@ -462,6 +462,7 @@ void Plane::update_navigation()
// we've reached the RTL point, see if we have a landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
mission.set_force_resume(true);
set_mode(mode_auto, ModeReason::UNKNOWN);
}
@ -474,6 +475,7 @@ void Plane::update_navigation()
// Go directly to the landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
mission.set_force_resume(true);
set_mode(mode_auto, ModeReason::UNKNOWN);
}
@ -582,7 +584,16 @@ void Plane::update_alt()
}
#endif
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
float target_alt = relative_target_altitude_cm();
if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) {
// ensure we do the initial climb in RTL. We add an extra
// 10m in the demanded height to push TECS to climb
// quickly
target_alt = MAX(target_alt, prev_WP_loc.alt + (g2.rtl_climb_min+10)*100);
}
SpdHgt_Controller->update_pitch_throttle(target_alt,
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,

15
ArduPlane/Parameters.cpp
View File

@ -460,10 +460,10 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: THR_FAILSAFE
// @DisplayName: Throttle and RC Failsafe Enable
// @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver.
// @Values: 0:Disabled,1:Enabled
// @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver. A value of 2 means that the RC input won't be used when throttle goes below the THR_FS_VALUE, but it won't trigger a failsafe
// @Values: 0:Disabled,1:Enabled,2:EnabledNoFailsafe
// @User: Standard
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", 1),
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", int(ThrFailsafe::Enabled)),
// @Param: THR_FS_VALUE
@ -1262,6 +1262,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
// @Param: RTL_CLIMB_MIN
// @DisplayName: RTL minimum climb
// @Description: The vehicle will climb this many m during the initial climb portion of the RTL. During this time the roll will be limited to LEVEL_ROLL_LIMIT degrees.
// @Units: m
// @Range: 0 30
// @Increment: 1
// @User: Standard
AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
AP_GROUPEND
};

3
ArduPlane/Parameters.h
View File

@ -569,6 +569,9 @@ public:
AP_Float fwd_thr_batt_voltage_max;
AP_Float fwd_thr_batt_voltage_min;
AP_Int8 fwd_thr_batt_idx;
// min initial climb in RTL
AP_Int16 rtl_climb_min;
};
extern const AP_Param::Info var_info[];

10
ArduPlane/Plane.h
View File

@ -746,6 +746,10 @@ private:
uint32_t last_trim_save;
} auto_trim;
struct {
bool done_climb;
} rtl;
// last time home was updated while disarmed
uint32_t last_home_update_ms;
@ -1052,6 +1056,12 @@ private:
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel");
enum class ThrFailsafe {
Disabled = 0,
Enabled = 1,
EnabledNoFS = 2
};
public:
void mavlink_delay_cb();
void failsafe_check(void);

2
ArduPlane/RC_Channel.cpp
View File

@ -18,7 +18,7 @@ int8_t RC_Channels_Plane::flight_mode_channel_number() const
bool RC_Channels_Plane::has_valid_input() const
{
if (plane.failsafe.rc_failsafe) {
if (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe) {
return false;
}
if (plane.failsafe.throttle_counter != 0) {

15
ArduPlane/mode_rtl.cpp
View File

@ -8,6 +8,7 @@ bool ModeRTL::_enter()
plane.auto_navigation_mode = true;
plane.prev_WP_loc = plane.current_loc;
plane.do_RTL(plane.get_RTL_altitude());
plane.rtl.done_climb = false;
return true;
}
@ -17,5 +18,19 @@ void ModeRTL::update()
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
if (plane.g2.rtl_climb_min > 0) {
/*
when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT
until we have climbed by RTL_CLIMB_MIN meters
*/
if (!plane.rtl.done_climb && (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min) {
plane.rtl.done_climb = true;
}
if (!plane.rtl.done_climb) {
plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
}
}
}

2
ArduPlane/mode_takeoff.cpp
View File

@ -65,7 +65,7 @@ void ModeTakeoff::update()
{
if (!takeoff_started) {
// see if we will skip takeoff as already flying
if (plane.is_flying() && plane.ahrs.groundspeed() > 3) {
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && plane.ahrs.groundspeed() > 3) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling");
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = plane.current_loc;

33
ArduPlane/navigation.cpp
View File

@ -36,8 +36,9 @@ void Plane::loiter_angle_update(void)
const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
const bool reached_target = reached_loiter_target();
if (loiter.sum_cd == 0 && !reached_loiter_target()) {
if (loiter.sum_cd == 0 && !reached_target) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
@ -53,7 +54,35 @@ void Plane::loiter_angle_update(void)
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.sum_cd += loiter_delta_cd * loiter.direction;
if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
bool reached_target_alt = false;
if (reached_target) {
// once we reach the position target we start checking the
// altitude target
bool terrain_status_ok = false;
#if AP_TERRAIN_AVAILABLE
/*
if doing terrain following then we check against terrain
target, fetch the terrain information
*/
float altitude_agl = 0;
if (target_altitude.terrain_following) {
if (terrain.status() == AP_Terrain::TerrainStatusOK &&
terrain.height_above_terrain(altitude_agl, true)) {
terrain_status_ok = true;
}
}
if (terrain_status_ok &&
fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) {
reached_target_alt = true;
} else
#endif
if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) {
reached_target_alt = true;
}
}
if (reached_target_alt) {
loiter.reached_target_alt = true;
loiter.unable_to_acheive_target_alt = false;
loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;

4
ArduPlane/quadplane.cpp
View File

@ -2367,6 +2367,10 @@ void QuadPlane::setup_target_position(void)
// setup vertical speed and acceleration
pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control->set_max_accel_z(pilot_accel_z);
// setup horizontal speed and acceleration
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
}
/*

6
ArduPlane/radio.cpp
View File

@ -179,6 +179,8 @@ void Plane::read_radio()
{
if (!rc().read_input()) {
control_failsafe();
airspeed_nudge_cm = 0;
throttle_nudge = 0;
return;
}
@ -280,7 +282,7 @@ void Plane::control_failsafe()
}
}
if(g.throttle_fs_enabled == 0) {
if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) {
return;
}
@ -373,7 +375,7 @@ bool Plane::trim_radio()
*/
bool Plane::rc_throttle_value_ok(void) const
{
if (!g.throttle_fs_enabled) {
if (ThrFailsafe(g.throttle_fs_enabled.get()) == ThrFailsafe::Disabled) {
return true;
}
if (channel_throttle->get_reverse()) {

10
ArduPlane/version.h
View File

@ -6,17 +6,13 @@
#include "ap_version.h"
#define THISFIRMWARE "ArduPlane V4.0.6beta1"
#define THISFIRMWARE "ArduPlane V4.0.6"
// the following line is parsed by the autotest scripts
#define FIRMWARE_VERSION 4,0,6,FIRMWARE_VERSION_TYPE_BETA
#define FIRMWARE_VERSION 4,0,6,FIRMWARE_VERSION_TYPE_OFFICIAL
#define FW_MAJOR 4
#define FW_MINOR 0
#define FW_PATCH 6
#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA
#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL