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

Merge branch 'ArduPilot:master' into nd210-pressure-sensor

This commit is contained in:
zzaurak 2024-08-18 15:43:11 +02:00 committed by GitHub
parent b6cb9bc3e5 1fe21b6120
commit c006e58ae2
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1078 changed files with 66989 additions and 34006 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.github/workflows/cygwin_build.yml vendored
View File

@ -202,6 +202,7 @@ jobs:
PATH: /usr/bin:$(cygpath ${SYSTEMROOT})/system32
shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}'
run: >-
git config --global --add safe.directory /cygdrive/d/a/${GITHUB_REPOSITORY#$GITHUB_REPOSITORY_OWNER/}/${GITHUB_REPOSITORY#$GITHUB_REPOSITORY_OWNER/} &&
export PATH=/usr/local/bin:/usr/bin:$(cygpath ${SYSTEMROOT})/system32 &&
source ~/ccache.conf &&
Tools/scripts/cygwin_build.sh &&

2
.github/workflows/macos_build.yml vendored
View File

@ -188,5 +188,7 @@ jobs:
echo $PATH
./waf configure --board ${{matrix.config}}
./waf
./waf configure --board ${{matrix.config}} --debug
./waf
ccache -s
ccache -z

172
.github/workflows/qurt_build.yml vendored Normal file
View File

@ -0,0 +1,172 @@
name: QURT Build
on:
push:
paths-ignore:
# remove other vehicles
- 'AntennaTracker/**'
- 'Blimp/**'
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
- 'Tools/CHDK-Script/**'
- 'Tools/CPUInfo/**'
- 'Tools/CodeStyle/**'
- 'Tools/FilterTestTool/**'
- 'Tools/Frame_params/**'
- 'Tools/GIT_Test/**'
- 'Tools/Hello/**'
- 'Tools/IO_Firmware/**'
- 'Tools/Linux_HAL_Essentials/**'
- 'Tools/LogAnalyzer/**'
- 'Tools/Pozyx/**'
- 'Tools/PrintVersion.py'
- 'Tools/Replay/**'
- 'Tools/UDP_Proxy/**'
- 'Tools/Vicon/**'
- 'Tools/bootloaders/**'
- 'Tools/completion/**'
- 'Tools/debug/**'
- 'Tools/environment_install/**'
- 'Tools/geotag/**'
- 'Tools/gittools/**'
- 'Tools/mavproxy_modules/**'
- 'Tools/simulink/**'
- 'Tools/vagrant/**'
# Discard python file from Tools/scripts as not used
- 'Tools/scripts/**.py'
- 'Tools/scripts/build_sizes/**'
- 'Tools/scripts/build_tests/**'
- 'Tools/scripts/CAN/**'
- 'Tools/scripts/signing/**'
# Remove autotest
- 'Tools/autotest/**'
# Remove markdown files as irrelevant
- '**.md'
# Remove dotfile at root directory
- './.dir-locals.el'
- './.dockerignore'
- './.editorconfig'
- './.flake8'
- './.gitattributes'
- './.github'
- './.gitignore'
- './.pre-commit-config.yaml'
- './.pydevproject'
- './.valgrind-suppressions'
- './.valgrindrc'
- 'Dockerfile'
- 'Vagrantfile'
- 'Makefile'
# Remove some directories check
- '.vscode/**'
- '.github/ISSUE_TEMPLATE/**'
# Remove change on other workflows
- '.github/workflows/test_environment.yml'
pull_request:
paths-ignore:
# remove other vehicles
- 'AntennaTracker/**'
- 'Blimp/**'
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
- 'Tools/bootloaders/**'
- 'Tools/CHDK-Script/**'
- 'Tools/CodeStyle/**'
- 'Tools/completion/**'
- 'Tools/CPUInfo/**'
- 'Tools/debug/**'
- 'Tools/environment_install/**'
- 'Tools/FilterTestTool/**'
- 'Tools/Frame_params/**'
- 'Tools/geotag/**'
- 'Tools/GIT_Test/**'
- 'Tools/gittools/**'
- 'Tools/Hello/**'
- 'Tools/IO_Firmware/**'
- 'Tools/Linux_HAL_Essentials/**'
- 'Tools/LogAnalyzer/**'
- 'Tools/mavproxy_modules/**'
- 'Tools/Pozyx/**'
- 'Tools/PrintVersion.py'
- 'Tools/Replay/**'
- 'Tools/simulink/**'
- 'Tools/UDP_Proxy/**'
- 'Tools/vagrant/**'
- 'Tools/Vicon/**'
# Discard python file from Tools/scripts as not used
- 'Tools/scripts/**.py'
- 'Tools/scripts/build_sizes/**'
- 'Tools/scripts/build_tests/**'
- 'Tools/scripts/CAN/**'
- 'Tools/scripts/signing/**'
# Remove autotest
- 'Tools/autotest/**'
# Remove markdown files as irrelevant
- '**.md'
# Remove dotfile at root directory
- './.dir-locals.el'
- './.dockerignore'
- './.editorconfig'
- './.flake8'
- './.gitattributes'
- './.github'
- './.gitignore'
- './.pre-commit-config.yaml'
- './.pydevproject'
- './.valgrind-suppressions'
- './.valgrindrc'
- 'Dockerfile'
- 'Vagrantfile'
- 'Makefile'
# Remove some directories check
- '.vscode/**'
- '.github/ISSUE_TEMPLATE/**'
# Remove change on other workflows
- '.github/workflows/test_environment.yml'
workflow_dispatch:
concurrency:
group: ci-${{github.workflow}}-${{ github.ref }}
cancel-in-progress: true
jobs:
build:
if: github.repository == 'ArduPilot/ardupilot'
runs-on: 'ardupilot-qurt'
steps:
- uses: actions/checkout@v4
with:
submodules: 'recursive'
- name: Build QURT
run: |
./waf configure --board QURT
./waf copter
./waf plane
./waf rover
cp -a build/QURT/bin/arducopter build/QURT/ArduPilot.so
cp -a build/QURT/bin/arducopter build/QURT/ArduPilot_Copter.so
cp -a build/QURT/bin/arduplane build/QURT/ArduPilot_Plane.so
cp -a build/QURT/bin/ardurover build/QURT/ArduPilot_Rover.so
- name: Archive build
uses: actions/upload-artifact@v3
with:
name: qurt-binaries
path: |
build/QURT/ardupilot
build/QURT/ArduPilot_Copter.so
build/QURT/ArduPilot_Plane.so
build/QURT/ArduPilot_Rover.so
build/QURT/ArduPilot.so
retention-days: 7

40
.github/workflows/test_environment.yml vendored
View File

@ -79,7 +79,7 @@ jobs:
with:
submodules: 'recursive'
- name: test install environment ${{matrix.os}}.${{matrix.name}}
timeout-minutes: 45
timeout-minutes: 60
env:
DISABLE_MAVNATIVE: True
DEBIAN_FRONTEND: noninteractive
@ -103,7 +103,7 @@ jobs:
*"archlinux"*)
cp /etc/skel/.bashrc /root
cp /etc/skel/.bashrc /github/home
git config --global --add safe.directory /__w/ardupilot/ardupilot
git config --global --add safe.directory ${GITHUB_WORKSPACE}
Tools/environment_install/install-prereqs-arch.sh -qy
;;
esac
@ -123,6 +123,7 @@ jobs:
./waf rover
- name: test build Chibios ${{matrix.os}}.${{matrix.name}}
if: matrix.os != 'alpine'
env:
DISABLE_MAVNATIVE: True
DEBIAN_FRONTEND: noninteractive
@ -141,3 +142,38 @@ jobs:
git config --global --add safe.directory /__w/ardupilot/ardupilot
./waf configure --board CubeOrange
./waf plane
build-alpine: # special case for alpine as it doesn't have bash by default
runs-on: ubuntu-22.04
container:
image: alpine:latest
options: --privileged
steps:
- name: Install Git
timeout-minutes: 30
env:
DEBIAN_FRONTEND: noninteractive
TZ: Europe/Paris
run: |
apk update && apk add --no-cache git
- uses: actions/checkout@v4
with:
submodules: 'recursive'
- name: test install environment alpine
timeout-minutes: 60
env:
DISABLE_MAVNATIVE: True
TZ: Europe/Paris
SKIP_AP_GIT_CHECK: 1
run: |
PATH="/github/home/.local/bin:$PATH"
Tools/environment_install/install-prereqs-alpine.sh
- name: test build STIL alpine
env:
DISABLE_MAVNATIVE: True
TZ: Europe/Paris
run: |
git config --global --add safe.directory ${GITHUB_WORKSPACE}
git config --global --add safe.directory /__w/ardupilot/ardupilot
./waf configure
./waf rover

1
.github/workflows/test_scripts.yml vendored
View File

@ -30,4 +30,5 @@ jobs:
CI_BUILD_TARGET: ${{matrix.config}}
shell: bash
run: |
git config --global --add safe.directory ${GITHUB_WORKSPACE}
Tools/scripts/build_ci.sh

2
AntennaTracker/GCS_Mavlink.cpp
View File

@ -289,7 +289,9 @@ static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
};
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
MSG_RC_CHANNELS,
#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
#endif
};
static const ap_message STREAM_EXTRA1_msgs[] = {
MSG_ATTITUDE,

50
AntennaTracker/ReleaseNotes.txt
View File

@ -1,5 +1,53 @@
Antenna Tracker Release Notes:
------------------------------
------------------------------------------------------------------
Release 4.5.5 1st Aug 2024
No changes from 4.5.5-beta2
------------------------------------------------------------------
Release 4.5.5-beta2 27 July 2024
Changes from 4.5.5-beta1
1) Board specific enhancements and bug fixes
- CubeRed's second core disabled at boot to avoid spurious writes to RAM
- CubeRed bootloader's dual endpoint update method fixed
------------------------------------------------------------------
Release 4.5.5-beta1 1st July 2024
Changes from 4.5.4
1) Board specific enhancements and bug fixes
- fixed IOMCU transmission errors when using bdshot
- update relay parameter names on various boards
- add ASP5033 airspeed in minimal builds
- added RadiolinkPIX6
- fix Aocoda-RC H743Dual motor issue
- use ICM45686 as an ICM20649 alternative in CubeRedPrimary
2) System level minor enhancements and bug fixes
- correct use-after-free in script statistics
- added arming check for eeprom full
- fixed a block logging issue which caused log messages to be dropped
- enable Socket SO_REUSEADDR on LwIP
- removed IST8310 overrun message
- added Siyi ZT6 support
- added BTFL sidebar symbols to the OSD
- added CRSF extended link stats to the OSD
- use the ESC with the highest RPM in the OSD when only one can be displayed
- support all Tramp power levels on high power VTXs
- emit jump count in missions even if no limit
- improve the bitmask indicating persistent parameters on bootloader flash
- fix duplicate error condition in the MicroStrain7
5) Other minor enhancements and bug fixes
- specify pymonocypher version in more places
- added DroneCAN dependencies to custom builds
------------------------------------------------------------------
Release 4.5.4 12th June 2024
Changes from 4.5.3

8
AntennaTracker/config.h
View File

@ -3,14 +3,6 @@
#include "defines.h"
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
// this avoids a very common config error
#define ENABLE ENABLED
#define DISABLE DISABLED
#ifndef MAV_SYSTEM_ID
// use 2 for antenna tracker by default
# define MAV_SYSTEM_ID 2

2
ArduCopter/APM_Config.h
View File

@ -4,8 +4,6 @@
//#define LOGGING_ENABLED DISABLED // disable logging to save 11K of flash space
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
//#define RANGEFINDER_ENABLED DISABLED // disable rangefinder to save 1k of flash
//#define PARACHUTE DISABLED // disable parachute release to save 1k of flash
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
//#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support
//#define MODE_AUTO_ENABLED DISABLED // disable auto mode support

42
ArduCopter/AP_Arming.cpp
View File

@ -238,11 +238,15 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
}
#else
if (copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD ||
copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL ||
copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) {
switch (copter.g2.frame_class.get()) {
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
case AP_Motors::MOTOR_FRAME_HELI:
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS");
return false;
default:
break;
}
#endif // HELI_FRAME
@ -257,6 +261,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
return false;
break;
case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
#if AP_RANGEFINDER_ENABLED
if (!copter.rangefinder_state.enabled || !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "no rangefinder");
return false;
@ -266,6 +271,9 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM");
return false;
}
#else
check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "rangefinder not in firmware");
#endif
break;
case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
// these checks are done in AP_Arming
@ -337,10 +345,10 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
{
// check if fence requires GPS
bool fence_requires_gps = false;
#if AP_FENCE_ENABLED
#if AP_FENCE_ENABLED
// if circular or polygon fence is enabled we need GPS
fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
#endif
#endif
// check if flight mode requires GPS
bool mode_requires_gps = copter.flightmode->requires_GPS() || fence_requires_gps || (copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE);
@ -439,12 +447,12 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
// check if fence requires GPS
bool fence_requires_gps = false;
#if AP_FENCE_ENABLED
#if AP_FENCE_ENABLED
// if circular or polygon fence is enabled we need GPS
fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
#endif
#endif
if (mode_requires_gps) {
if (mode_requires_gps || copter.option_is_enabled(Copter::FlightOption::REQUIRE_POSITION_FOR_ARMING)) {
if (!copter.position_ok()) {
// vehicle level position estimate checks
check_failed(display_failure, "Need Position Estimate");
@ -597,11 +605,11 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
// check throttle
if (check_enabled(ARMING_CHECK_RC)) {
#if FRAME_CONFIG == HELI_FRAME
#if FRAME_CONFIG == HELI_FRAME
const char *rc_item = "Collective";
#else
#else
const char *rc_item = "Throttle";
#endif
#endif
// check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto
if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) {
// above top of deadband is too always high
@ -610,12 +618,12 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
return false;
}
// in manual modes throttle must be at zero
#if FRAME_CONFIG != HELI_FRAME
#if FRAME_CONFIG != HELI_FRAME
if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
return false;
}
#endif
#endif
}
}
@ -801,12 +809,8 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
if (copter.flightmode == &copter.mode_autotune) {
copter.mode_autotune.save_tuning_gains();
} else {
copter.mode_autotune.reset();
}
// Possibly save auto tuned parameters
copter.mode_autotune.autotune.disarmed(copter.flightmode == &copter.mode_autotune);
#endif
// we are not in the air

42
ArduCopter/Copter.cpp
View File

@ -164,7 +164,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#endif
SCHED_TASK(auto_disarm_check, 10, 50, 27),
SCHED_TASK(auto_trim, 10, 75, 30),
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
SCHED_TASK(read_rangefinder, 20, 100, 33),
#endif
#if HAL_PROXIMITY_ENABLED
@ -446,15 +446,41 @@ bool Copter::has_ekf_failsafed() const
return failsafe.ekf;
}
// get target location (for use by scripting)
bool Copter::get_target_location(Location& target_loc)
{
return flightmode->get_wp(target_loc);
}
/*
update_target_location() acts as a wrapper for set_target_location
*/
bool Copter::update_target_location(const Location &old_loc, const Location &new_loc)
{
/*
by checking the caller has provided the correct old target
location we prevent a race condition where the user changes mode
or commands a different target in the controlling lua script
*/
Location next_WP_loc;
flightmode->get_wp(next_WP_loc);
if (!old_loc.same_loc_as(next_WP_loc) ||
old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
return false;
}
return set_target_location(new_loc);
}
#endif // AP_SCRIPTING_ENABLED
// returns true if vehicle is landing. Only used by Lua scripts
// returns true if vehicle is landing.
bool Copter::is_landing() const
{
return flightmode->is_landing();
}
// returns true if vehicle is taking off. Only used by Lua scripts
// returns true if vehicle is taking off.
bool Copter::is_taking_off() const
{
return flightmode->is_taking_off();
@ -552,14 +578,17 @@ void Copter::ten_hz_logging_loop()
if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_EKF_POS();
}
if (should_log(MASK_LOG_MOTBATT)) {
if ((FRAME_CONFIG == HELI_FRAME) || should_log(MASK_LOG_MOTBATT)) {
// always write motors log if we are a heli
motors->Log_Write();
}
if (should_log(MASK_LOG_RCIN)) {
logger.Write_RCIN();
#if AP_RSSI_ENABLED
if (rssi.enabled()) {
logger.Write_RSSI();
}
#endif
}
if (should_log(MASK_LOG_RCOUT)) {
logger.Write_RCOUT();
@ -579,9 +608,6 @@ void Copter::ten_hz_logging_loop()
g2.beacon.log();
#endif
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
#if AP_WINCH_ENABLED
if (should_log(MASK_LOG_ANY)) {
g2.winch.write_log();
@ -631,7 +657,7 @@ void Copter::three_hz_loop()
// check for deadreckoning failsafe
failsafe_deadreckon_check();
// update ch6 in flight tuning
//update transmitter based in flight tuning
tuning();
// check if avoidance should be enabled based on alt

35
ArduCopter/Copter.h
View File

@ -121,7 +121,7 @@
#if AP_TERRAIN_AVAILABLE
# include <AP_Terrain/AP_Terrain.h>
#endif
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
# include <AP_RangeFinder/AP_RangeFinder.h>
#endif
@ -258,6 +258,7 @@ private:
// helper function to get inertially interpolated rangefinder height.
bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
#if AP_RANGEFINDER_ENABLED
class SurfaceTracking {
public:
@ -292,6 +293,7 @@ private:
bool valid_for_logging; // true if we have a desired target altitude
bool reset_target; // true if target should be reset because of change in surface being tracked
} surface_tracking;
#endif
#if AP_RPM_ENABLED
AP_RPM rpm_sensor;
@ -519,7 +521,7 @@ private:
#endif
// Parachute release
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
AP_Parachute parachute;
#endif
@ -626,6 +628,7 @@ private:
DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1
DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2
RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4
REQUIRE_POSITION_FOR_ARMING = (1<<3), // 8
};
// returns true if option is enabled for this vehicle
bool option_is_enabled(FlightOption option) const {
@ -670,6 +673,8 @@ private:
#if AP_SCRIPTING_ENABLED
#if MODE_GUIDED_ENABLED == ENABLED
bool start_takeoff(float alt) override;
bool get_target_location(Location& target_loc) override;
bool update_target_location(const Location &old_loc, const Location &new_loc) override;
bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override;
bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override;
bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
@ -825,6 +830,29 @@ private:
void set_land_complete(bool b);
void set_land_complete_maybe(bool b);
void update_throttle_mix();
bool get_force_flying() const;
#if HAL_LOGGING_ENABLED
enum class LandDetectorLoggingFlag : uint16_t {
LANDED = 1U << 0,
LANDED_MAYBE = 1U << 1,
LANDING = 1U << 2,
STANDBY_ACTIVE = 1U << 3,
WOW = 1U << 4,
RANGEFINDER_BELOW_2M = 1U << 5,
DESCENT_RATE_LOW = 1U << 6,
ACCEL_STATIONARY = 1U << 7,
LARGE_ANGLE_ERROR = 1U << 8,
LARGE_ANGLE_REQUEST = 1U << 8,
MOTOR_AT_LOWER_LIMIT = 1U << 9,
THROTTLE_MIX_AT_MIN = 1U << 10,
};
struct {
uint32_t last_logged_ms;
uint32_t last_logged_count;
uint16_t last_logged_flags;
} land_detector;
void Log_LDET(uint16_t logging_flags, uint32_t land_detector_count);
#endif
#if AP_LANDINGGEAR_ENABLED
// landing_gear.cpp
@ -854,9 +882,6 @@ private:
void Log_Write_Data(LogDataID id, float value);
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max);
void Log_Video_Stabilisation();
#if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void);
#endif
void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target);
void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate);
void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);

2
ArduCopter/GCS_Copter.cpp
View File

@ -91,7 +91,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

32
ArduCopter/GCS_Mavlink.cpp
View File

@ -531,7 +531,9 @@ static const ap_message STREAM_POSITION_msgs[] = {
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
MSG_SERVO_OUTPUT_RAW,
MSG_RC_CHANNELS,
#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
#endif
};
static const ap_message STREAM_EXTRA1_msgs[] = {
MSG_ATTITUDE,
@ -793,7 +795,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
case MAV_CMD_NAV_VTOL_TAKEOFF:
return handle_MAV_CMD_NAV_TAKEOFF(packet);
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
case MAV_CMD_DO_PARACHUTE:
return handle_MAV_CMD_DO_PARACHUTE(packet);
#endif
@ -971,7 +973,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_comman
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet)
{
// configure or release parachute
@ -1217,6 +1219,7 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
// ensure thrust field is not ignored
if (throttle_ignore) {
// The throttle input is not defined
return;
}
@ -1235,6 +1238,18 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
}
}
Vector3f ang_vel_body;
if (!roll_rate_ignore && !pitch_rate_ignore && !yaw_rate_ignore) {
ang_vel_body.x = packet.body_roll_rate;
ang_vel_body.y = packet.body_pitch_rate;
ang_vel_body.z = packet.body_yaw_rate;
} else if (!(roll_rate_ignore && pitch_rate_ignore && yaw_rate_ignore)) {
// The body rates are ill-defined
// input is not valid so stop
copter.mode_guided.init(true);
return;
}
// check if the message's thrust field should be interpreted as a climb rate or as thrust
const bool use_thrust = copter.mode_guided.set_attitude_target_provides_thrust();
@ -1256,18 +1271,7 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
}
}
Vector3f ang_vel;
if (!roll_rate_ignore) {
ang_vel.x = packet.body_roll_rate;
}
if (!pitch_rate_ignore) {
ang_vel.y = packet.body_pitch_rate;
}
if (!yaw_rate_ignore) {
ang_vel.z = packet.body_yaw_rate;
}
copter.mode_guided.set_angle(attitude_quat, ang_vel,
copter.mode_guided.set_angle(attitude_quat, ang_vel_body,
climb_rate_or_thrust, use_thrust);
}

47
ArduCopter/Log.cpp
View File

@ -39,11 +39,15 @@ void Copter::Log_Write_Control_Tuning()
target_climb_rate_cms = pos_control->get_vel_target_z_cms();
}
// get surface tracking alts
float desired_rangefinder_alt;
#if AP_RANGEFINDER_ENABLED
if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) {
desired_rangefinder_alt = AP::logger().quiet_nan();
}
#else
// get surface tracking alts
desired_rangefinder_alt = AP::logger().quiet_nan();
#endif
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
@ -56,7 +60,11 @@ void Copter::Log_Write_Control_Tuning()
inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f,
baro_alt : baro_alt,
desired_rangefinder_alt : desired_rangefinder_alt,
#if AP_RANGEFINDER_ENABLED
rangefinder_alt : surface_tracking.get_dist_for_logging(),
#else
rangefinder_alt : AP::logger().quiet_nanf(),
#endif
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,
climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()) // float -> int16_t
@ -301,31 +309,6 @@ void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitu
#endif
}
#if FRAME_CONFIG == HELI_FRAME
struct PACKED log_Heli {
LOG_PACKET_HEADER;
uint64_t time_us;
float desired_rotor_speed;
float main_rotor_speed;
float governor_output;
float control_output;
};
// Write an helicopter packet
void Copter::Log_Write_Heli()
{
struct log_Heli pkt_heli = {
LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
time_us : AP_HAL::micros64(),
desired_rotor_speed : motors->get_desired_rotor_speed(),
main_rotor_speed : motors->get_main_rotor_speed(),
governor_output : motors->get_governor_output(),
control_output : motors->get_control_output(),
};
logger.WriteBlock(&pkt_heli, sizeof(pkt_heli));
}
#endif
// guided position target logging
struct PACKED log_Guided_Position_Target {
LOG_PACKET_HEADER;
@ -482,18 +465,6 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
"DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },
// @LoggerMessage: HELI
// @Description: Helicopter related messages
// @Field: TimeUS: Time since system startup
// @Field: DRRPM: Desired rotor speed
// @Field: ERRPM: Estimated rotor speed
// @Field: Gov: Governor Output
// @Field: Throt: Throttle output
#if FRAME_CONFIG == HELI_FRAME
{ LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qffff", "TimeUS,DRRPM,ERRPM,Gov,Throt", "s----", "F----" , true },
#endif
// @LoggerMessage: SIDD
// @Description: System ID data
// @Field: TimeUS: Time since system startup

12
ArduCopter/Parameters.cpp
View File

@ -453,7 +453,7 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(relay, "RELAY", AP_Relay),
#endif
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
// @Group: CHUTE_
// @Path: ../libraries/AP_Parachute/AP_Parachute.cpp
GOBJECT(parachute, "CHUTE_", AP_Parachute),
@ -634,11 +634,13 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission),
#endif
#if AP_RSSI_ENABLED
// @Group: RSSI_
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
GOBJECT(rssi, "RSSI_", AP_RSSI),
#endif
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
// @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
GOBJECT(rangefinder, "RNGFND", RangeFinder),
@ -1000,11 +1002,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: FLIGHT_OPTIONS
// @DisplayName: Flight mode options
// @Description: Flight mode specific options
// @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss
// @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss, 3:Require position for arming
// @User: Advanced
AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0),
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
// @Param: RNGFND_FILT
// @DisplayName: Rangefinder filter
// @Description: Rangefinder filter to smooth distance. Set to zero to disable filtering
@ -1028,6 +1030,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// ACRO_PR_RATE (47), ACRO_Y_RATE (48), PILOT_Y_RATE (49) and PILOT_Y_EXPO (50) moved to command model class
#if AP_RANGEFINDER_ENABLED
// @Param: SURFTRAK_MODE
// @DisplayName: Surface Tracking Mode
// @Description: set which surface to track in surface tracking
@ -1035,6 +1038,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND),
#endif
// @Param: FS_DR_ENABLE
// @DisplayName: DeadReckon Failsafe Action

2
ArduCopter/Parameters.h
View File

@ -652,7 +652,7 @@ public:
AP_Int32 flight_options;
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
AP_Float rangefinder_filt;
#endif

18
ArduCopter/RC_Channel.cpp
View File

@ -76,7 +76,8 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
// the following functions do not need to be initialised:
case AUX_FUNC::ALTHOLD:
case AUX_FUNC::AUTO:
case AUX_FUNC::AUTOTUNE:
case AUX_FUNC::AUTOTUNE_MODE:
case AUX_FUNC::AUTOTUNE_TEST_GAINS:
case AUX_FUNC::BRAKE:
case AUX_FUNC::CIRCLE:
case AUX_FUNC::DRIFT:
@ -128,6 +129,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
case AUX_FUNC::FORCEFLYING:
case AUX_FUNC::CUSTOM_CONTROLLER:
case AUX_FUNC::WEATHER_VANE_ENABLE:
case AUX_FUNC::TRANSMITTER_TUNING:
run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
break;
default:
@ -258,7 +260,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
break;
#endif
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
case AUX_FUNC::RANGEFINDER:
// enable or disable the rangefinder
if ((ch_flag == AuxSwitchPos::HIGH) &&
@ -290,9 +292,12 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
#endif
#if AUTOTUNE_ENABLED == ENABLED
case AUX_FUNC::AUTOTUNE:
case AUX_FUNC::AUTOTUNE_MODE:
do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag);
break;
case AUX_FUNC::AUTOTUNE_TEST_GAINS:
copter.mode_autotune.autotune.do_aux_function(ch_flag);
break;
#endif
case AUX_FUNC::LAND:
@ -311,7 +316,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
do_aux_function_change_mode(Mode::Number::FOLLOW, ch_flag);
break;
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
case AUX_FUNC::PARACHUTE_ENABLE:
// Parachute enable/disable
copter.parachute.enabled(ch_flag == AuxSwitchPos::HIGH);
@ -544,6 +549,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
break;
}
#if AP_RANGEFINDER_ENABLED
case AUX_FUNC::SURFACE_TRACKING:
switch (ch_flag) {
case AuxSwitchPos::LOW:
@ -557,6 +563,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
break;
}
break;
#endif
case AUX_FUNC::FLIGHTMODE_PAUSE:
switch (ch_flag) {
@ -646,6 +653,9 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
break;
}
#endif
case AUX_FUNC::TRANSMITTER_TUNING:
// do nothing, used in tuning.cpp for transmitter based tuning
break;
default:
return RC_Channel::do_aux_function(ch_option, ch_flag);

58
ArduCopter/ReleaseNotes.txt
View File

@ -1,5 +1,61 @@
ArduPilot Copter Release Notes:
-------------------------------
------------------------------------------------------------------
Release 4.5.5 1st Aug 2024
No changes from 4.5.5-beta2
------------------------------------------------------------------
Release 4.5.5-beta2 27 July 2024
Changes from 4.5.5-beta1
1) Board specific enhancements and bug fixes
- CubeRed's second core disabled at boot to avoid spurious writes to RAM
- CubeRed bootloader's dual endpoint update method fixed
------------------------------------------------------------------
Release 4.5.5-beta1 1st July 2024
Changes from 4.5.4
1) Board specific enhancements and bug fixes
- fixed IOMCU transmission errors when using bdshot
- update relay parameter names on various boards
- add ASP5033 airspeed in minimal builds
- added RadiolinkPIX6
- fix Aocoda-RC H743Dual motor issue
- use ICM45686 as an ICM20649 alternative in CubeRedPrimary
2) System level minor enhancements and bug fixes
- correct use-after-free in script statistics
- added arming check for eeprom full
- fixed a block logging issue which caused log messages to be dropped
- enable Socket SO_REUSEADDR on LwIP
- removed IST8310 overrun message
- added Siyi ZT6 support
- added BTFL sidebar symbols to the OSD
- added CRSF extended link stats to the OSD
- use the ESC with the highest RPM in the OSD when only one can be displayed
- support all Tramp power levels on high power VTXs
- emit jump count in missions even if no limit
- improve the bitmask indicating persistent parameters on bootloader flash
- fix duplicate error condition in the MicroStrain7
3) AHRS / EKF fixes
- fix infinite climb bug when using EK3_OGN_HGT_MASK
4) Copter specific changes
- fix MAV_CMD_CONDITION_YAW with relative angle
5) Other minor enhancements and bug fixes
- specify pymonocypher version in more places
- added DroneCAN dependencies to custom builds
------------------------------------------------------------------
Release 4.5.4 12th June 2024
Changes from 4.5.3

10
ArduCopter/config.h
View File

@ -70,10 +70,6 @@
// Rangefinder
//
#ifndef RANGEFINDER_ENABLED
# define RANGEFINDER_ENABLED ENABLED
#endif
#ifndef RANGEFINDER_FILT_DEFAULT
# define RANGEFINDER_FILT_DEFAULT 0.5f // filter for rangefinder distance
#endif
@ -124,12 +120,6 @@
# define AUTOTUNE_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Parachute release
#ifndef PARACHUTE
# define PARACHUTE HAL_PARACHUTE_ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Nav-Guided - allows external nav computer to control vehicle
#ifndef AC_NAV_GUIDED

10
ArduCopter/crash_check.cpp
View File

@ -36,13 +36,13 @@ void Copter::crash_check()
}
// exit immediately if in force flying
if (force_flying && !flightmode->is_landing()) {
if (get_force_flying() && !flightmode->is_landing()) {
crash_counter = 0;
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) {
if (!flightmode->crash_check_enabled()) {
crash_counter = 0;
return;
}
@ -229,7 +229,7 @@ void Copter::yaw_imbalance_check()
}
}
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
// Code to detect a crash main ArduCopter code
#define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute
@ -273,7 +273,7 @@ void Copter::parachute_check()
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) {
if (!flightmode->crash_check_enabled()) {
control_loss_count = 0;
return;
}
@ -369,4 +369,4 @@ void Copter::parachute_manual_release()
parachute_release();
}
#endif // PARACHUTE == ENABLED
#endif // HAL_PARACHUTE_ENABLED

11
ArduCopter/fence.cpp
View File

@ -10,8 +10,10 @@ void Copter::fence_check()
{
const uint8_t orig_breaches = fence.get_breaches();
bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete || !motors->armed();
// check for new breaches; new_breaches is bitmask of fence types breached
const uint8_t new_breaches = fence.check();
const uint8_t new_breaches = fence.check(is_landing_or_landed);
// we still don't do anything when disarmed, but we do check for fence breaches.
// fence pre-arm check actually checks if any fence has been breached
@ -24,7 +26,7 @@ void Copter::fence_check()
if (new_breaches) {
if (!copter.ap.land_complete) {
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached");
fence.print_fence_message("breached", new_breaches);
}
// if the user wants some kind of response and motors are armed
@ -81,7 +83,10 @@ void Copter::fence_check()
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
} else if (orig_breaches) {
} else if (orig_breaches && fence.get_breaches() == 0) {
if (!copter.ap.land_complete) {
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
}
// record clearing of breach
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
}

7
ArduCopter/heli.cpp
View File

@ -42,11 +42,13 @@ void Copter::check_dynamic_flight(void)
moving = (motors->get_throttle() > 0.8f || ahrs.pitch_sensor < -1500);
}
#if AP_RANGEFINDER_ENABLED
if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) {
// when we are more than 2m from the ground with good
// rangefinder lock consider it to be dynamic flight
moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200);
}
#endif
if (moving) {
// if moving for 2 seconds, set the dynamic flight flag
@ -104,8 +106,9 @@ void Copter::update_heli_control_dynamics(void)
bool Copter::should_use_landing_swash() const
{
if (flightmode->has_manual_throttle() ||
flightmode->mode_number() == Mode::Number::DRIFT) {
// manual modes always uses full swash range
flightmode->mode_number() == Mode::Number::DRIFT ||
attitude_control->get_inverted_flight()) {
// manual modes or modes using inverted flight uses full swash range
return false;
}
if (flightmode->is_landing()) {

78
ArduCopter/land_detector.cpp
View File

@ -22,7 +22,7 @@ void Copter::update_land_and_crash_detectors()
update_land_detector();
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
// check parachute
parachute_check();
#endif
@ -44,6 +44,13 @@ void Copter::update_land_detector()
// range finder : tend to be problematic at very short distances
// input throttle : in slow land the input throttle may be only slightly less than hover
#if HAL_LOGGING_ENABLED
uint16_t logging_flags = 0;
#define SET_LOG_FLAG(condition, flag) if (condition) { logging_flags |= (uint16_t)flag; }
#else
#define SET_LOG_FLAG(condition, flag)
#endif
if (!motors->armed()) {
// if disarmed, always landed.
set_land_complete(true);
@ -74,11 +81,12 @@ void Copter::update_land_detector()
// check if landing
const bool landing = flightmode->is_landing();
SET_LOG_FLAG(landing, LandDetectorLoggingFlag::LANDING);
bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll()) < M_PI/2.0f)
#if MODE_AUTOROTATE_ENABLED == ENABLED
|| (flightmode->mode_number() == Mode::Number::AUTOROTATE && motors->get_below_land_min_coll())
#endif
|| ((!force_flying || landing) && motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f);
|| ((!get_force_flying() || landing) && motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f);
bool throttle_mix_at_min = true;
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
@ -91,6 +99,8 @@ void Copter::update_land_detector()
throttle_mix_at_min = true;
}
#endif
SET_LOG_FLAG(motor_at_lower_limit, LandDetectorLoggingFlag::MOTOR_AT_LOWER_LIMIT);
SET_LOG_FLAG(throttle_mix_at_min, LandDetectorLoggingFlag::THROTTLE_MIX_AT_MIN);
uint8_t land_detector_scalar = 1;
#if AP_LANDINGGEAR_ENABLED
@ -103,19 +113,24 @@ void Copter::update_land_detector()
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
SET_LOG_FLAG(large_angle_request, LandDetectorLoggingFlag::LARGE_ANGLE_REQUEST);
// check for large external disturbance - angle error over 30 degrees
const float angle_error = attitude_control->get_att_error_angle_deg();
bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
SET_LOG_FLAG(large_angle_error, LandDetectorLoggingFlag::LARGE_ANGLE_ERROR);
// check that the airframe is not accelerating (not falling or braking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);
SET_LOG_FLAG(accel_stationary, LandDetectorLoggingFlag::ACCEL_STATIONARY);
// check that vertical speed is within 1m/s of zero
bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100.0 * LAND_DETECTOR_VEL_Z_MAX * land_detector_scalar;
SET_LOG_FLAG(descent_rate_low, LandDetectorLoggingFlag::DESCENT_RATE_LOW);
// if we have a healthy rangefinder only allow landing detection below 2 meters
bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);
SET_LOG_FLAG(rangefinder_check, LandDetectorLoggingFlag::RANGEFINDER_BELOW_2M);
// if we have weight on wheels (WoW) or ambiguous unknown. never no WoW
#if AP_LANDINGGEAR_ENABLED
@ -123,6 +138,7 @@ void Copter::update_land_detector()
#else
const bool WoW_check = true;
#endif
SET_LOG_FLAG(WoW_check, LandDetectorLoggingFlag::WOW);
if (motor_at_lower_limit && throttle_mix_at_min && !large_angle_request && !large_angle_error && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) {
// landed criteria met - increment the counter and check if we've triggered
@ -138,8 +154,53 @@ void Copter::update_land_detector()
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
#if HAL_LOGGING_ENABLED
// @LoggerMessage: LDET
// @Description: Land Detector State
// @Field: TimeUS: Time since system startup
// @Field: Flags: boolean state flags
// @FieldBitmaskEnum: Flags: Copter::LandDetectorLoggingFlag
// @Field: Count: landing_detector pass count
SET_LOG_FLAG(ap.land_complete, LandDetectorLoggingFlag::LANDED);
SET_LOG_FLAG(ap.land_complete_maybe, LandDetectorLoggingFlag::LANDED_MAYBE);
SET_LOG_FLAG(standby_active, LandDetectorLoggingFlag::STANDBY_ACTIVE);
Log_LDET(logging_flags, land_detector_count);
#undef SET_LOG_FLAG
#endif
}
#if HAL_LOGGING_ENABLED
void Copter::Log_LDET(uint16_t logging_flags, uint32_t detector_count)
{
// do not log if no change:
if (logging_flags == land_detector.last_logged_flags &&
detector_count == land_detector.last_logged_count) {
return;
}
// do not log more than 50Hz:
const auto now = AP_HAL::millis();
if (now - land_detector.last_logged_ms < 20) {
return;
}
land_detector.last_logged_count = detector_count;
land_detector.last_logged_flags = logging_flags;
land_detector.last_logged_ms = now;
AP::logger().WriteStreaming(
"LDET",
"TimeUS," "Flags," "Count",
"s" "-" "-",
"F" "-" "-",
"Q" "H" "I",
AP_HAL::micros64(),
logging_flags,
land_detector_count
);
}
#endif
// set land_complete flag and disarm motors if disarm-on-land is configured
void Copter::set_land_complete(bool b)
{
@ -251,7 +312,7 @@ void Copter::update_throttle_mix()
// check if landing
const bool landing = flightmode->is_landing();
if (((large_angle_request || force_flying) && !landing) || large_angle_error || accel_moving || descent_not_demanded) {
if (((large_angle_request || get_force_flying()) && !landing) || large_angle_error || accel_moving || descent_not_demanded) {
attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio());
} else {
attitude_control->set_throttle_mix_min();
@ -259,3 +320,14 @@ void Copter::update_throttle_mix()
}
#endif
}
// helper function for force flying flag
bool Copter::get_force_flying() const
{
#if FRAME_CONFIG == HELI_FRAME
if (attitude_control->get_inverted_flight()) {
return true;
}
#endif
return force_flying;
}

2
ArduCopter/landing_gear.cpp
View File

@ -14,6 +14,7 @@ void Copter::landinggear_update()
int32_t height_cm = flightmode->get_alt_above_ground_cm();
// use rangefinder if available
#if AP_RANGEFINDER_ENABLED
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::Status::NotConnected:
case RangeFinder::Status::NoData:
@ -31,6 +32,7 @@ void Copter::landinggear_update()
height_cm = rangefinder_state.alt_cm_filt.get();
break;
}
#endif // AP_RANGEFINDER_ENABLED
landinggear.update(height_cm * 0.01f); // convert cm->m for update call
}

19
ArduCopter/mode.cpp
View File

@ -347,6 +347,20 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
return false;
}
#if AP_FENCE_ENABLED
// may not be allowed to change mode if recovering from fence breach
if (!ignore_checks &&
fence.enabled() &&
fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
fence.get_breaches() &&
motors->armed() &&
get_control_mode_reason() == ModeReason::FENCE_BREACHED &&
!ap.land_complete) {
mode_change_failed(new_flightmode, "in fence recovery");
return false;
}
#endif
if (!new_flightmode->init(ignore_checks)) {
mode_change_failed(new_flightmode, "init failed");
return false;
@ -371,10 +385,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
#endif
#if AP_FENCE_ENABLED
if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
}
#endif
#if AP_CAMERA_ENABLED
@ -420,7 +436,10 @@ bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
// called at 100hz or more
void Copter::update_flight_mode()
{
#if AP_RANGEFINDER_ENABLED
surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used
#endif
attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation
flightmode->run();
}

35
ArduCopter/mode.h
View File

@ -128,6 +128,7 @@ public:
virtual bool allows_save_trim() const { return false; }
virtual bool allows_autotune() const { return false; }
virtual bool allows_flip() const { return false; }
virtual bool crash_check_enabled() const { return true; }
#if FRAME_CONFIG == HELI_FRAME
virtual bool allows_inverted() const { return false; };
@ -185,6 +186,9 @@ public:
virtual bool pause() { return false; };
virtual bool resume() { return false; };
// handle situations where the vehicle is on the ground waiting for takeoff
void make_safe_ground_handling(bool force_throttle_unlimited = false);
// true if weathervaning is allowed in the current mode
#if WEATHERVANE_ENABLED == ENABLED
virtual bool allows_weathervaning() const { return false; }
@ -196,7 +200,6 @@ protected:
bool is_disarmed_or_landed() const;
void zero_throttle_and_relax_ac(bool spool_up = false);
void zero_throttle_and_hold_attitude();
void make_safe_ground_handling(bool force_throttle_unlimited = false);
// Return stopping point as a location with above origin alt frame
Location get_stopping_point() const;
@ -418,6 +421,7 @@ public:
void air_mode_aux_changed();
bool allows_save_trim() const override { return true; }
bool allows_flip() const override { return true; }
bool crash_check_enabled() const override { return false; }
protected:
@ -471,7 +475,9 @@ public:
}
bool allows_autotune() const override { return true; }
bool allows_flip() const override { return true; }
#if FRAME_CONFIG == HELI_FRAME
bool allows_inverted() const override { return true; };
#endif
protected:
const char *name() const override { return "ALT_HOLD"; }
@ -499,6 +505,9 @@ public:
bool allows_arming(AP_Arming::Method method) const override;
bool is_autopilot() const override { return true; }
bool in_guided_mode() const override { return _mode == SubMode::NAVGUIDED || _mode == SubMode::NAV_SCRIPT_TIME; }
#if FRAME_CONFIG == HELI_FRAME
bool allows_inverted() const override { return true; };
#endif
// Auto modes
enum class SubMode : uint8_t {
@ -589,12 +598,13 @@ protected:
private:
enum class Options : int32_t {
enum class Option : int32_t {
AllowArming = (1 << 0U),
AllowTakeOffWithoutRaisingThrottle = (1 << 1U),
IgnorePilotYaw = (1 << 2U),
AllowWeatherVaning = (1 << 7U),
};
bool option_is_enabled(Option option) const;
// Enter auto rtl pseudo mode
bool enter_auto_rtl(ModeReason reason);
@ -643,7 +653,7 @@ private:
void do_set_home(const AP_Mission::Mission_Command& cmd);
void do_roi(const AP_Mission::Mission_Command& cmd);
void do_mount_control(const AP_Mission::Mission_Command& cmd);
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
void do_parachute(const AP_Mission::Mission_Command& cmd);
#endif
#if AP_WINCH_ENABLED
@ -786,18 +796,12 @@ public:
bool allows_arming(AP_Arming::Method method) const override { return false; }
bool is_autopilot() const override { return false; }
void save_tuning_gains();
void reset();
AutoTune autotune;
protected:
const char *name() const override { return "AUTOTUNE"; }
const char *name4() const override { return "ATUN"; }
private:
AutoTune autotune;
};
#endif
@ -903,6 +907,7 @@ public:
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return false; };
bool is_autopilot() const override { return false; }
bool crash_check_enabled() const override { return false; }
protected:
@ -1250,6 +1255,10 @@ public:
bool has_user_takeoff(bool must_navigate) const override { return true; }
bool allows_autotune() const override { return true; }
#if FRAME_CONFIG == HELI_FRAME
bool allows_inverted() const override { return true; };
#endif
#if AC_PRECLAND_ENABLED
void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
#endif
@ -1529,6 +1538,10 @@ private:
// point while following our path home. If we take too long we
// may choose to land the vehicle.
uint32_t path_follow_last_pop_fail_ms;
// backup last popped point so that it can be restored to the path
// if vehicle exits SmartRTL mode before reaching home. invalid if zero
Vector3f dest_NED_backup;
};

2
ArduCopter/mode_althold.cpp
View File

@ -87,8 +87,10 @@ void ModeAltHold::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);

75
ArduCopter/mode_auto.cpp
View File

@ -202,15 +202,23 @@ void ModeAuto::set_submode(SubMode new_submode)
}
}
bool ModeAuto::option_is_enabled(Option option) const
{
return ((copter.g2.auto_options & (uint32_t)option) != 0);
}
bool ModeAuto::allows_arming(AP_Arming::Method method) const
{
return ((copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0) && !auto_RTL;
};
if (auto_RTL) {
return false;
}
return option_is_enabled(Option::AllowArming);
}
#if WEATHERVANE_ENABLED == ENABLED
bool ModeAuto::allows_weathervaning() const
{
return (copter.g2.auto_options & (uint32_t)Options::AllowWeatherVaning) != 0;
return option_is_enabled(Option::AllowWeatherVaning);
}
#endif
@ -480,11 +488,6 @@ void ModeAuto::land_start()
copter.landinggear.deploy_for_landing();
#endif
#if AP_FENCE_ENABLED
// disable the fence on landing
copter.fence.auto_disable_fence_for_landing();
#endif
// reset flag indicating if pilot has applied roll or pitch inputs during landing
copter.ap.land_repo_active = false;
@ -638,7 +641,7 @@ void PayloadPlace::start_descent()
// returns true if pilot's yaw input should be used to adjust vehicle's heading
bool ModeAuto::use_pilot_yaw(void) const
{
const bool allow_yaw_option = (copter.g2.auto_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0;
const bool allow_yaw_option = !option_is_enabled(Option::IgnorePilotYaw);
const bool rtl_allow_yaw = (_mode == SubMode::RTL) && copter.mode_rtl.use_pilot_yaw();
const bool landing = _mode == SubMode::LAND;
return allow_yaw_option || rtl_allow_yaw || landing;
@ -780,18 +783,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
do_mount_control(cmd);
break;
case MAV_CMD_DO_FENCE_ENABLE:
#if AP_FENCE_ENABLED
if (cmd.p1 == 0) { //disable
copter.fence.enable(false);
gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
} else { //enable fence
copter.fence.enable(true);
gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
}
#endif //AP_FENCE_ENABLED
break;
#if AC_NAV_GUIDED == ENABLED
case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits
do_guided_limits(cmd);
@ -1039,7 +1030,7 @@ void ModeAuto::takeoff_run()
{
// if the user doesn't want to raise the throttle we can set it automatically
// note that this can defeat the disarm check on takeoff
if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) {
if (option_is_enabled(Option::AllowTakeOffWithoutRaisingThrottle)) {
copter.set_auto_armed(true);
}
auto_takeoff.run();
@ -1197,8 +1188,10 @@ void ModeAuto::loiter_to_alt_run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
@ -1253,13 +1246,21 @@ void PayloadPlace::run()
const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds
const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed
// Vertical thrust is taken from the attitude controller before angle boost is added
auto &g2 = copter.g2;
const auto &g = copter.g;
auto &inertial_nav = copter.inertial_nav;
auto *attitude_control = copter.attitude_control;
const auto &pos_control = copter.pos_control;
const auto &wp_nav = copter.wp_nav;
// Vertical thrust is taken from the attitude controller before angle boost is added
const float thrust_level = attitude_control->get_throttle_in();
const uint32_t now_ms = AP_HAL::millis();
// relax position target if we might be landed
// if we discover we've landed then immediately release the load:
if (copter.ap.land_complete || copter.ap.land_complete_maybe) {
pos_control->soften_for_landing_xy();
switch (state) {
case State::FlyToLocation:
// this is handled in wp_run()
@ -1287,7 +1288,9 @@ void PayloadPlace::run()
switch (state) {
case State::FlyToLocation:
case State::Descent_Start:
gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str);
gcs().send_text(MAV_SEVERITY_INFO, "%s Abort: Gripper Open", prefix_str);
// Descent_Start has not run so we must also initalise descent_start_altitude_cm
descent_start_altitude_cm = inertial_nav.get_position_z_up_cm();
state = State::Done;
break;
case State::Descent:
@ -1305,12 +1308,6 @@ void PayloadPlace::run()
}
#endif
auto &inertial_nav = copter.inertial_nav;
auto &g2 = copter.g2;
const auto &g = copter.g;
const auto &wp_nav = copter.wp_nav;
const auto &pos_control = copter.pos_control;
switch (state) {
case State::FlyToLocation:
if (copter.wp_nav->reached_wp_destination()) {
@ -1442,8 +1439,7 @@ void PayloadPlace::run()
copter.flightmode->land_run_horizontal_control();
// update altitude target and call position controller
pos_control->land_at_climb_rate_cm(-descent_speed_cms, true);
pos_control->update_z_controller();
return;
break;
case State::Release:
case State::Releasing:
case State::Delay:
@ -1451,8 +1447,7 @@ void PayloadPlace::run()
copter.flightmode->land_run_horizontal_control();
// update altitude target and call position controller
pos_control->land_at_climb_rate_cm(0.0, false);
pos_control->update_z_controller();
return;
break;
case State::Ascent:
case State::Done:
float vel = 0.0;
@ -1460,6 +1455,7 @@ void PayloadPlace::run()
pos_control->input_pos_vel_accel_z(descent_start_altitude_cm, vel, 0.0);
break;
}
pos_control->update_z_controller();
}
#endif
@ -1921,15 +1917,20 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
if (cmd.content.speed.speed_type == 2) {
switch (cmd.content.speed.speed_type) {
case SPEED_TYPE_CLIMB_SPEED:
copter.wp_nav->set_speed_up(cmd.content.speed.target_ms * 100.0f);
desired_speed_override.up = cmd.content.speed.target_ms;
} else if (cmd.content.speed.speed_type == 3) {
break;
case SPEED_TYPE_DESCENT_SPEED:
copter.wp_nav->set_speed_down(cmd.content.speed.target_ms * 100.0f);
desired_speed_override.down = cmd.content.speed.target_ms;
} else {
break;
case SPEED_TYPE_AIRSPEED:
case SPEED_TYPE_GROUNDSPEED:
copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
desired_speed_override.xy = cmd.content.speed.target_ms;
break;
}
}
}

38
ArduCopter/mode_autotune.cpp
View File

@ -38,30 +38,20 @@ void AutoTune::run()
// apply SIMPLE mode transform to pilot inputs
copter.update_simple_mode();
// reset target lean angles and heading while landed
if (copter.ap.land_complete) {
// we are landed, shut down
float target_climb_rate = get_pilot_desired_climb_rate_cms();
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// disarm when the landing detector says we've landed and spool state is ground idle
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm(AP_Arming::Method::LANDED);
}
copter.attitude_control->reset_rate_controller_I_terms_smoothly();
copter.attitude_control->reset_yaw_target_and_rate();
float target_roll, target_pitch, target_yaw_rate;
get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate);
// if not armed set throttle to zero and exit immediately
if (copter.ap.land_complete) {
copter.flightmode->make_safe_ground_handling();
return;
}
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
copter.pos_control->relax_z_controller(0.0f);
copter.pos_control->update_z_controller();
} else {
// run autotune mode
AC_AutoTune::run();
}
}
@ -128,19 +118,9 @@ void ModeAutoTune::run()
autotune.run();
}
void ModeAutoTune::save_tuning_gains()
{
autotune.save_tuning_gains();
}
void ModeAutoTune::exit()
{
autotune.stop();
}
void ModeAutoTune::reset()
{
autotune.reset();
}
#endif // AUTOTUNE_ENABLED == ENABLED

4
ArduCopter/mode_circle.cpp
View File

@ -68,7 +68,7 @@ void ModeCircle::run()
}
// update the orbicular rate target based on pilot roll stick inputs
// skip if using CH6 tuning knob for circle rate
// skip if using transmitter based tuning knob for circle rate
if (g.radio_tuning != TUNING_CIRCLE_RATE) {
const float roll_stick = channel_roll->norm_input_dz(); // roll stick normalized -1 to 1
@ -114,8 +114,10 @@ void ModeCircle::run()
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
pos_control->update_z_controller();

2
ArduCopter/mode_flowhold.cpp
View File

@ -302,8 +302,10 @@ void ModeFlowHold::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);

30
ArduCopter/mode_guided.cpp
View File

@ -15,7 +15,7 @@ static uint32_t update_time_ms; // system time of last target update
struct {
uint32_t update_time_ms;
Quaternion attitude_quat;
Vector3f ang_vel;
Vector3f ang_vel_body;
float yaw_rate_cds;
float climb_rate_cms; // climb rate in cms. Used if use_thrust is false
float thrust; // thrust from -1 to 1. Used if use_thrust is true
@ -128,6 +128,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
// calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
int32_t alt_target_cm;
bool alt_target_terrain = false;
#if AP_RANGEFINDER_ENABLED
if (wp_nav->rangefinder_used_and_healthy() &&
wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
@ -138,7 +139,9 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
// provide target altitude as alt-above-terrain
alt_target_cm = takeoff_alt_cm;
alt_target_terrain = true;
} else {
} else
#endif
{
// interpret altitude as alt-above-home
Location target_loc = copter.current_loc;
target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);
@ -320,7 +323,7 @@ void ModeGuided::angle_control_start()
// initialise targets
guided_angle_state.update_time_ms = millis();
guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
guided_angle_state.ang_vel.zero();
guided_angle_state.ang_vel_body.zero();
guided_angle_state.climb_rate_cms = 0.0f;
guided_angle_state.yaw_rate_cds = 0.0f;
guided_angle_state.use_yaw_rate = false;
@ -638,13 +641,13 @@ bool ModeGuided::use_wpnav_for_position_control() const
}
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
// ang_vel: angular velocity (rad/s)
// attitude_quat: IF zero: ang_vel_body (body frame angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and body frame angular velocity
// ang_vel_body: body frame angular velocity (rad/s)
// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
// IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust)
void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel_body, float climb_rate_cms_or_thrust, bool use_thrust)
{
// check we are in velocity control mode
if (guided_mode != SubMode::Angle) {
@ -652,7 +655,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
}
guided_angle_state.attitude_quat = attitude_quat;
guided_angle_state.ang_vel = ang_vel;
guided_angle_state.ang_vel_body = ang_vel_body;
guided_angle_state.use_thrust = use_thrust;
if (use_thrust) {
@ -671,7 +674,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
#if HAL_LOGGING_ENABLED
// log target
copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel_body, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
#endif
}
@ -682,6 +685,9 @@ void ModeGuided::takeoff_run()
auto_takeoff.run();
if (auto_takeoff.complete && !takeoff_complete) {
takeoff_complete = true;
#if AP_FENCE_ENABLED
copter.fence.auto_enable_fence_after_takeoff();
#endif
#if AP_LANDINGGEAR_ENABLED
// optionally retract landing gear
copter.landinggear.retract_after_takeoff();
@ -946,7 +952,7 @@ void ModeGuided::angle_control_run()
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
guided_angle_state.ang_vel.zero();
guided_angle_state.ang_vel_body.zero();
climb_rate_cms = 0.0f;
if (guided_angle_state.use_thrust) {
// initialise vertical velocity controller
@ -985,9 +991,9 @@ void ModeGuided::angle_control_run()
// call attitude controller
if (guided_angle_state.attitude_quat.is_zero()) {
attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel.x) * 100.0f, ToDeg(guided_angle_state.ang_vel.y) * 100.0f, ToDeg(guided_angle_state.ang_vel.z) * 100.0f);
attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel_body.x) * 100.0f, ToDeg(guided_angle_state.ang_vel_body.y) * 100.0f, ToDeg(guided_angle_state.ang_vel_body.z) * 100.0f);
} else {
attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel);
attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel_body);
}
// call position controller

5
ArduCopter/mode_land.cpp
View File

@ -41,11 +41,6 @@ bool ModeLand::init(bool ignore_checks)
copter.landinggear.deploy_for_landing();
#endif
#if AP_FENCE_ENABLED
// disable the fence on landing
copter.fence.auto_disable_fence_for_landing();
#endif
#if AC_PRECLAND_ENABLED
// initialise precland state machine
copter.precland_statemachine.init();

2
ArduCopter/mode_loiter.cpp
View File

@ -191,8 +191,10 @@ void ModeLoiter::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);

2
ArduCopter/mode_poshold.cpp
View File

@ -158,8 +158,10 @@ void ModePosHold::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);

10
ArduCopter/mode_rtl.cpp
View File

@ -255,11 +255,6 @@ void ModeRTL::descent_start()
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
#endif
#if AP_FENCE_ENABLED
// disable the fence on landing
copter.fence.auto_disable_fence_for_landing();
#endif
}
// rtl_descent_run - implements the final descent to the RTL_ALT
@ -347,11 +342,6 @@ void ModeRTL::land_start()
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
#endif
#if AP_FENCE_ENABLED
// disable the fence on landing
copter.fence.auto_disable_fence_for_landing();
#endif
}
bool ModeRTL::is_landing() const

16
ArduCopter/mode_smart_rtl.cpp
View File

@ -35,6 +35,14 @@ bool ModeSmartRTL::init(bool ignore_checks)
// perform cleanup required when leaving smart_rtl
void ModeSmartRTL::exit()
{
// restore last point if we hadn't reached it
if (smart_rtl_state == SubMode::PATH_FOLLOW && !dest_NED_backup.is_zero()) {
if (!g2.smart_rtl.add_point(dest_NED_backup)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SmartRTL: lost one point");
}
}
dest_NED_backup.zero();
g2.smart_rtl.cancel_request_for_thorough_cleanup();
}
@ -83,10 +91,16 @@ void ModeSmartRTL::path_follow_run()
{
// if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) {
Vector3f dest_NED;
// clear destination backup so that it cannot be restored
dest_NED_backup.zero();
// this pop_point can fail if the IO task currently has the
// path semaphore.
Vector3f dest_NED;
if (g2.smart_rtl.pop_point(dest_NED)) {
// backup destination in case we exit smart_rtl mode and need to restore it to the path
dest_NED_backup = dest_NED;
path_follow_last_pop_fail_ms = 0;
if (g2.smart_rtl.get_num_points() == 0) {
// this is the very last point, add 2m to the target alt and move to pre-land state

2
ArduCopter/mode_sport.cpp
View File

@ -108,8 +108,10 @@ void ModeSport::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);

4
ArduCopter/mode_stabilize_heli.cpp
View File

@ -77,8 +77,8 @@ void ModeStabilize_Heli::run()
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// output pilot's throttle - note that TradHeli does not used angle-boost
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
// output pilot's throttle
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}
#endif //HELI_FRAME

8
ArduCopter/mode_zigzag.cpp
View File

@ -244,9 +244,11 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
if (maintain_target) {
const Vector3f& wp_dest = wp_nav->get_wp_destination();
loiter_nav->init_target(wp_dest.xy());
#if AP_RANGEFINDER_ENABLED
if (wp_nav->origin_and_destination_are_terrain_alt()) {
copter.surface_tracking.set_target_alt_cm(wp_dest.z);
}
#endif
} else {
loiter_nav->init_target();
}
@ -377,8 +379,10 @@ void ModeZigZag::manual_control()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
@ -454,9 +458,11 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
// if we have a downward facing range finder then use terrain altitude targets
terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
if (terrain_alt) {
#if AP_RANGEFINDER_ENABLED
if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
}
#endif
} else {
next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
}
@ -506,9 +512,11 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
// if we have a downward facing range finder then use terrain altitude targets
terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
if (terrain_alt) {
#if AP_RANGEFINDER_ENABLED
if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
}
#endif
} else {
next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
}

9
ArduCopter/sensors.cpp
View File

@ -8,9 +8,9 @@ void Copter::read_barometer(void)
baro_alt = barometer.get_altitude() * 100.0f;
}
#if AP_RANGEFINDER_ENABLED
void Copter::init_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
rangefinder.init(ROTATION_PITCH_270);
rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
@ -19,13 +19,11 @@ void Copter::init_rangefinder(void)
// upward facing range finder
rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
#endif
}
// return rangefinder altitude in centimeters
void Copter::read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED
rangefinder.update();
rangefinder_state.update();
@ -36,9 +34,8 @@ void Copter::read_rangefinder(void)
g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
}
#endif
#endif
}
#endif // AP_RANGEFINDER_ENABLED
// return true if rangefinder_alt can be used
bool Copter::rangefinder_alt_ok() const
@ -73,7 +70,7 @@ void Copter::update_rangefinder_terrain_offset()
// helper function to get inertially interpolated rangefinder height.
bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const
{
#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED
#if AP_RANGEFINDER_ENABLED
return rangefinder_state.get_rangefinder_height_interpolated_cm(ret);
#else
return false;

9
ArduCopter/surface_tracking.cpp
View File

@ -1,10 +1,11 @@
#include "Copter.h"
#if AP_RANGEFINDER_ENABLED
// update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling
// level measured using the range finder.
void Copter::SurfaceTracking::update_surface_offset()
{
#if RANGEFINDER_ENABLED == ENABLED
// check for timeout
const uint32_t now_ms = millis();
const bool timeout = (now_ms - last_update_ms) > SURFACE_TRACKING_TIMEOUT_MS;
@ -41,10 +42,6 @@ void Copter::SurfaceTracking::update_surface_offset()
reset_target = true;
}
}
#else
copter.pos_control->set_pos_offset_z_cm(0);
copter.pos_control->set_pos_offset_target_z_cm(0);
#endif
}
@ -111,3 +108,5 @@ void Copter::SurfaceTracking::set_surface(Surface new_surface)
surface = new_surface;
reset_target = true;
}
#endif // AP_RANGEFINDER_ENABLED

6
ArduCopter/system.cpp
View File

@ -27,8 +27,10 @@ void Copter::init_ardupilot()
// initialise battery monitor
battery.init();
#if AP_RSSI_ENABLED
// Init RSSI
rssi.init();
#endif
barometer.init();
@ -52,9 +54,11 @@ void Copter::init_ardupilot()
init_rc_in(); // sets up rc channels from radio
#if AP_RANGEFINDER_ENABLED
// initialise surface to be tracked in SurfaceTracking
// must be before rc init to not override initial switch position
surface_tracking.init((SurfaceTracking::Surface)copter.g2.surftrak_mode.get());
#endif
// allocate the motors class
allocate_motors();
@ -133,7 +137,7 @@ void Copter::init_ardupilot()
barometer.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate();
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
// initialise rangefinder
init_rangefinder();
#endif

2
ArduCopter/terrain.cpp
View File

@ -8,7 +8,7 @@ void Copter::terrain_update()
// tell the rangefinder our height, so it can go into power saving
// mode if available
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
float height;
if (terrain.height_above_terrain(height, true)) {
rangefinder.set_estimated_terrain_height(height);

14
ArduCopter/toy_mode.cpp
View File

@ -431,7 +431,7 @@ void ToyMode::update()
if (set_and_remember_mode(Mode::Number::ALT_HOLD, ModeReason::TOY_MODE)) {
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: ALT_HOLD update arm");
#if AP_FENCE_ENABLED
copter.fence.enable(false);
copter.fence.enable(false, AC_FENCE_ALL_FENCES);
#endif
if (!copter.arming.arm(AP_Arming::Method::MAVLINK)) {
// go back to LOITER
@ -460,7 +460,7 @@ void ToyMode::update()
#endif
} else if (copter.position_ok() && set_and_remember_mode(Mode::Number::LOITER, ModeReason::TOY_MODE)) {
#if AP_FENCE_ENABLED
copter.fence.enable(true);
copter.fence.enable(true, AC_FENCE_ALL_FENCES);
#endif
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: LOITER update");
}
@ -644,14 +644,14 @@ void ToyMode::update()
if (new_mode != copter.flightmode->mode_number()) {
load_test.running = false;
#if AP_FENCE_ENABLED
copter.fence.enable(false);
copter.fence.enable(false, AC_FENCE_ALL_FENCES);
#endif
if (set_and_remember_mode(new_mode, ModeReason::TOY_MODE)) {
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: mode %s", copter.flightmode->name4());
// force fence on in all GPS flight modes
#if AP_FENCE_ENABLED
if (copter.flightmode->requires_GPS()) {
copter.fence.enable(true);
copter.fence.enable(true, AC_FENCE_ALL_FENCES);
}
#endif
} else {
@ -662,7 +662,7 @@ void ToyMode::update()
set_and_remember_mode(Mode::Number::LAND, ModeReason::TOY_MODE);
#if AP_FENCE_ENABLED
if (copter.landing_with_GPS()) {
copter.fence.enable(true);
copter.fence.enable(true, AC_FENCE_ALL_FENCES);
}
#endif
}
@ -801,7 +801,7 @@ void ToyMode::action_arm(void)
if (needs_gps && copter.arming.gps_checks(false)) {
#if AP_FENCE_ENABLED
// we want GPS and checks are passing, arm and enable fence
copter.fence.enable(true);
copter.fence.enable(true, AC_FENCE_ALL_FENCES);
#endif
copter.arming.arm(AP_Arming::Method::RUDDER);
if (!copter.motors->armed()) {
@ -817,7 +817,7 @@ void ToyMode::action_arm(void)
} else {
#if AP_FENCE_ENABLED
// non-GPS mode
copter.fence.enable(false);
copter.fence.enable(false, AC_FENCE_ALL_FENCES);
#endif
copter.arming.arm(AP_Arming::Method::RUDDER);
if (!copter.motors->armed()) {

79
ArduCopter/tuning.cpp
View File

@ -1,15 +1,20 @@
#include "Copter.h"
/*
* Function to update various parameters in flight using the ch6 tuning knob
* Function to update various parameters in flight using the TRANSMITTER_TUNING channel knob
* This should not be confused with the AutoTune feature which can be found in control_autotune.cpp
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// tuning - updates parameters based on the ch6 TRANSMITTER_TUNING channel knob's position
// should be called at 3.3hz
void Copter::tuning()
{
const RC_Channel *rc6 = rc().channel(CH_6);
const RC_Channel *rc_tuning = rc().find_channel_for_option(RC_Channel::AUX_FUNC::TRANSMITTER_TUNING);
// exit immediately if tuning channel is not set
if (rc_tuning == nullptr) {
return;
}
// exit immediately if the tuning function is not set or min and max are both zero
if ((g.radio_tuning <= 0) || (is_zero(g2.tuning_min.get()) && is_zero(g2.tuning_max.get()))) {
@ -17,17 +22,13 @@ void Copter::tuning()
}
// exit immediately when radio failsafe is invoked or transmitter has not been turned on
if (failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) {
if (failsafe.radio || failsafe.radio_counter != 0 || rc_tuning->get_radio_in() == 0) {
return;
}
// exit immediately if a function is assigned to channel 6
if ((RC_Channel::AUX_FUNC)rc6->option.get() != RC_Channel::AUX_FUNC::DO_NOTHING) {
return;
}
const uint16_t radio_in = rc_tuning->get_radio_in();
float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc_tuning->get_radio_min(), rc_tuning->get_radio_max());
const uint16_t radio_in = rc6->get_radio_in();
float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max());
#if HAL_LOGGING_ENABLED
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max);
#endif
@ -36,70 +37,70 @@ void Copter::tuning()
// Roll, Pitch tuning
case TUNING_STABILIZE_ROLL_PITCH_KP:
attitude_control->get_angle_roll_p().kP(tuning_value);
attitude_control->get_angle_pitch_p().kP(tuning_value);
attitude_control->get_angle_roll_p().set_kP(tuning_value);
attitude_control->get_angle_pitch_p().set_kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KP:
attitude_control->get_rate_roll_pid().kP(tuning_value);
attitude_control->get_rate_pitch_pid().kP(tuning_value);
attitude_control->get_rate_roll_pid().set_kP(tuning_value);
attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KI:
attitude_control->get_rate_roll_pid().kI(tuning_value);
attitude_control->get_rate_pitch_pid().kI(tuning_value);
attitude_control->get_rate_roll_pid().set_kI(tuning_value);
attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KD:
attitude_control->get_rate_roll_pid().kD(tuning_value);
attitude_control->get_rate_pitch_pid().kD(tuning_value);
attitude_control->get_rate_roll_pid().set_kD(tuning_value);
attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
break;
// Yaw tuning
case TUNING_STABILIZE_YAW_KP:
attitude_control->get_angle_yaw_p().kP(tuning_value);
attitude_control->get_angle_yaw_p().set_kP(tuning_value);
break;
case TUNING_YAW_RATE_KP:
attitude_control->get_rate_yaw_pid().kP(tuning_value);
attitude_control->get_rate_yaw_pid().set_kP(tuning_value);
break;
case TUNING_YAW_RATE_KD:
attitude_control->get_rate_yaw_pid().kD(tuning_value);
attitude_control->get_rate_yaw_pid().set_kD(tuning_value);
break;
// Altitude and throttle tuning
case TUNING_ALTITUDE_HOLD_KP:
pos_control->get_pos_z_p().kP(tuning_value);
pos_control->get_pos_z_p().set_kP(tuning_value);
break;
case TUNING_THROTTLE_RATE_KP:
pos_control->get_vel_z_pid().kP(tuning_value);
pos_control->get_vel_z_pid().set_kP(tuning_value);
break;
case TUNING_ACCEL_Z_KP:
pos_control->get_accel_z_pid().kP(tuning_value);
pos_control->get_accel_z_pid().set_kP(tuning_value);
break;
case TUNING_ACCEL_Z_KI:
pos_control->get_accel_z_pid().kI(tuning_value);
pos_control->get_accel_z_pid().set_kI(tuning_value);
break;
case TUNING_ACCEL_Z_KD:
pos_control->get_accel_z_pid().kD(tuning_value);
pos_control->get_accel_z_pid().set_kD(tuning_value);
break;
// Loiter and navigation tuning
case TUNING_LOITER_POSITION_KP:
pos_control->get_pos_xy_p().kP(tuning_value);
pos_control->get_pos_xy_p().set_kP(tuning_value);
break;
case TUNING_VEL_XY_KP:
pos_control->get_vel_xy_pid().kP(tuning_value);
pos_control->get_vel_xy_pid().set_kP(tuning_value);
break;
case TUNING_VEL_XY_KI:
pos_control->get_vel_xy_pid().kI(tuning_value);
pos_control->get_vel_xy_pid().set_kI(tuning_value);
break;
case TUNING_WP_SPEED:
@ -126,15 +127,15 @@ void Copter::tuning()
break;
case TUNING_RATE_PITCH_FF:
attitude_control->get_rate_pitch_pid().ff(tuning_value);
attitude_control->get_rate_pitch_pid().set_ff(tuning_value);
break;
case TUNING_RATE_ROLL_FF:
attitude_control->get_rate_roll_pid().ff(tuning_value);
attitude_control->get_rate_roll_pid().set_ff(tuning_value);
break;
case TUNING_RATE_YAW_FF:
attitude_control->get_rate_yaw_pid().ff(tuning_value);
attitude_control->get_rate_yaw_pid().set_ff(tuning_value);
break;
#endif
@ -153,27 +154,27 @@ void Copter::tuning()
break;
case TUNING_RATE_PITCH_KP:
attitude_control->get_rate_pitch_pid().kP(tuning_value);
attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
break;
case TUNING_RATE_PITCH_KI:
attitude_control->get_rate_pitch_pid().kI(tuning_value);
attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
break;
case TUNING_RATE_PITCH_KD:
attitude_control->get_rate_pitch_pid().kD(tuning_value);
attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
break;
case TUNING_RATE_ROLL_KP:
attitude_control->get_rate_roll_pid().kP(tuning_value);
attitude_control->get_rate_roll_pid().set_kP(tuning_value);
break;
case TUNING_RATE_ROLL_KI:
attitude_control->get_rate_roll_pid().kI(tuning_value);
attitude_control->get_rate_roll_pid().set_kI(tuning_value);
break;
case TUNING_RATE_ROLL_KD:
attitude_control->get_rate_roll_pid().kD(tuning_value);
attitude_control->get_rate_roll_pid().set_kD(tuning_value);
break;
#if FRAME_CONFIG != HELI_FRAME
@ -183,7 +184,7 @@ void Copter::tuning()
#endif
case TUNING_RATE_YAW_FILT:
attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value);
attitude_control->get_rate_yaw_pid().set_filt_E_hz(tuning_value);
break;
case TUNING_SYSTEM_ID_MAGNITUDE:

8
ArduPlane/AP_Arming.cpp
View File

@ -370,12 +370,8 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
change_arm_state();
#if QAUTOTUNE_ENABLED
//save qautotune gains if enabled and success
if (plane.control_mode == &plane.mode_qautotune) {
plane.quadplane.qautotune.save_tuning_gains();
} else {
plane.quadplane.qautotune.reset();
}
// Possibly save auto tuned parameters
plane.quadplane.qautotune.disarmed(plane.control_mode == &plane.mode_qautotune);
#endif
// re-initialize speed variable used in AUTO and GUIDED for

14
ArduPlane/ArduPlane.cpp
View File

@ -31,7 +31,7 @@
All entries in this table must be ordered by priority.
This table is interleaved with the table presnet in each of the
This table is interleaved with the table present in each of the
vehicles to determine the order in which tasks are run. Convenience
methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
entries in this structure:
@ -81,7 +81,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63),
#endif
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66),
#if AP_RANGEFINDER_ENABLED
SCHED_TASK(read_rangefinder, 50, 100, 78),
#endif
#if AP_ICENGINE_ENABLED
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
#endif
@ -557,7 +559,7 @@ void Plane::update_alt()
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
parachute.set_sink_rate(auto_state.sink_rate);
#endif
@ -750,7 +752,9 @@ float Plane::tecs_hgt_afe(void)
float hgt_afe;
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
hgt_afe = height_above_target();
#if AP_RANGEFINDER_ENABLED
hgt_afe -= rangefinder_correction();
#endif
} else {
// when in normal flight we pass the hgt_afe as relative
// altitude to home
@ -829,7 +833,7 @@ bool Plane::set_target_location(const Location &target_loc)
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED
// set target location (for use by scripting)
// get target location (for use by scripting)
bool Plane::get_target_location(Location& target_loc)
{
switch (control_mode->mode_number()) {
@ -967,7 +971,11 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const
void Plane::precland_update(void)
{
// alt will be unused if we pass false through as the second parameter:
#if AP_RANGEFINDER_ENABLED
return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range);
#else
return g2.precland.update(0, false);
#endif
}
#endif

5
ArduPlane/Attitude.cpp
View File

@ -448,6 +448,10 @@ void Plane::stabilize()
}
/*
* Set the throttle output.
* This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc.
*/
void Plane::calc_throttle()
{
if (aparm.throttle_cruise <= 1) {
@ -458,6 +462,7 @@ void Plane::calc_throttle()
return;
}
// Read the TECS throttle output and set it to the throttle channel.
float commanded_throttle = TECS_controller.get_throttle_demand();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
}

30
ArduPlane/GCS_Mavlink.cpp
View File

@ -645,7 +645,9 @@ static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
MSG_SERVO_OUTPUT_RAW,
MSG_RC_CHANNELS,
#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
#endif
};
static const ap_message STREAM_EXTRA1_msgs[] = {
MSG_ATTITUDE,
@ -841,6 +843,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
return MAV_RESULT_DENIED;
}
#if AP_FENCE_ENABLED
// reject destination if outside the fence
if (!plane.fence.check_destination_within_fence(requested_position)) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
return MAV_RESULT_DENIED;
}
#endif
// location is valid load and set
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == &plane.mode_guided)) {
@ -864,12 +874,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
return MAV_RESULT_FAILED;
}
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// these are GUIDED mode commands that are RATE or slew enabled, so you can have more powerful control than default controls.
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet)
{
switch(packet.command) {
#if OFFBOARD_GUIDED == ENABLED
case MAV_CMD_GUIDED_CHANGE_SPEED: {
// command is only valid in guided mode
if (plane.control_mode != &plane.mode_guided) {
@ -998,14 +1007,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
plane.guided_state.target_heading_time_ms = AP_HAL::millis();
return MAV_RESULT_ACCEPTED;
}
#endif // OFFBOARD_GUIDED == ENABLED
}
// anything else ...
return MAV_RESULT_UNSUPPORTED;
}
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
@ -1017,11 +1023,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
case MAV_CMD_DO_REPOSITION:
return handle_command_int_do_reposition(packet);
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// special 'slew-enabled' guided commands here... for speed,alt, and direction commands
case MAV_CMD_GUIDED_CHANGE_SPEED:
case MAV_CMD_GUIDED_CHANGE_ALTITUDE:
case MAV_CMD_GUIDED_CHANGE_HEADING:
return handle_command_int_guided_slew_commands(packet);
#endif
#if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED
case MAV_CMD_DO_FOLLOW:
@ -1044,7 +1052,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
case MAV_CMD_DO_CHANGE_SPEED:
return handle_command_DO_CHANGE_SPEED(packet);
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
case MAV_CMD_DO_PARACHUTE:
return handle_MAV_CMD_DO_PARACHUTE(packet);
#endif
@ -1083,6 +1091,12 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
case MAV_CMD_SET_HAGL:
plane.handle_external_hagl(packet);
return MAV_RESULT_ACCEPTED;
#endif
default:
return GCS_MAVLINK::handle_command_int_packet(packet, msg);
}
@ -1166,7 +1180,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_co
return MAV_RESULT_ACCEPTED;
}
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet)
{
// configure or release parachute

2
ArduPlane/GCS_Plane.cpp
View File

@ -114,6 +114,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
}
#endif
#if AP_RANGEFINDER_ENABLED
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
@ -124,4 +125,5 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
}

12
ArduPlane/Log.cpp
View File

@ -121,7 +121,7 @@ void Plane::Log_Write_Control_Tuning()
logger.WriteBlock(&pkt, sizeof(pkt));
}
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
struct PACKED log_OFG_Guided {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -255,15 +255,17 @@ void Plane::Log_Write_RC(void)
{
logger.Write_RCIN();
logger.Write_RCOUT();
#if AP_RSSI_ENABLED
if (rssi.enabled()) {
logger.Write_RSSI();
}
#endif
Log_Write_AETR();
}
void Plane::Log_Write_Guided(void)
{
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (control_mode != &mode_guided) {
return;
}
@ -275,7 +277,7 @@ void Plane::Log_Write_Guided(void)
if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) {
Log_Write_OFG_Guided();
}
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
}
// incoming-to-vehicle mavlink COMMAND_INT can be logged
@ -431,7 +433,7 @@ const struct LogStructure Plane::log_structure[] = {
// @LoggerMessage: TSIT
// @Description: tailsitter speed scailing values
// @Field: TimeUS: Time since system startup
// @Field: Ts: throttle scailing used for tilt motors
// @Field: Ts: throttle scaling used for tilt motors
// @Field: Ss: speed scailing used for control surfaces method from Q_TAILSIT_GSCMSK
// @Field: Tmin: minimum output throttle caculated from disk thoery gain scale with Q_TAILSIT_MIN_VO
#if HAL_QUADPLANE_ENABLED
@ -481,7 +483,7 @@ const struct LogStructure Plane::log_structure[] = {
{ LOG_AETR_MSG, sizeof(log_AETR),
"AETR", "Qfffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true },
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// @LoggerMessage: OFG
// @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s.
// @Field: TimeUS: Time since system startup

37
ArduPlane/Parameters.cpp
View File

@ -142,13 +142,29 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: TKOFF_THR_MAX_T
// @DisplayName: Takeoff throttle maximum time
// @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff without an airspeed sensor. If an airspeed sensor is being used then the throttle is set to maximum until the takeoff airspeed is reached.
// @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff.
// @Units: s
// @Range: 0 10
// @Increment: 0.5
// @User: Standard
ASCALAR(takeoff_throttle_max_t, "TKOFF_THR_MAX_T", 4),
// @Param: TKOFF_THR_MIN
// @DisplayName: Takeoff minimum throttle
// @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_OPTIONS bit 0 is set. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead.
// @Units: %
// @Range: 0 100
// @Increment: 1
// @User: Standard
ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60),
// @Param: TKOFF_OPTIONS
// @DisplayName: Takeoff options
// @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes.
// @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor.
// @User: Advanced
ASCALAR(takeoff_options, "TKOFF_OPTIONS", 0),
// @Param: TKOFF_TDRAG_ELEV
// @DisplayName: Takeoff tail dragger elevator
// @Description: This parameter sets the amount of elevator to apply during the initial stage of a takeoff. It is used to hold the tail wheel of a taildragger on the ground during the initial takeoff stage to give maximum steering. This option should be combined with the TKOFF_TDRAG_SPD1 option and the GROUND_STEER_ALT option along with tuning of the ground steering controller. A value of zero means to bypass the initial "tail hold" stage of takeoff. Set to zero for hand and catapult launch. For tail-draggers you should normally set this to 100, meaning full up elevator during the initial stage of takeoff. For most tricycle undercarriage aircraft a value of zero will work well, but for some tricycle aircraft a small negative value (say around -20 to -30) will apply down elevator which will hold the nose wheel firmly on the ground during initial acceleration. Only use a negative value if you find that the nosewheel doesn't grip well during takeoff. Too much down elevator on a tricycle undercarriage may cause instability in steering as the plane pivots around the nosewheel. Add down elevator 10 percent at a time.
@ -296,6 +312,14 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Standard
ASCALAR(airspeed_max, "AIRSPEED_MAX", AIRSPEED_FBW_MAX),
// @Param: AIRSPEED_STALL
// @DisplayName: Stall airspeed
// @Description: If stall prevention is enabled this speed is used to calculate the minimum airspeed while banking. If this is set to 0 then the stall speed is assumed to be the minimum airspeed speed. Typically set slightly higher then true stall speed. Value is as an indicated (calibrated/apparent) airspeed.
// @Units: m/s
// @Range: 5 75
// @User: Standard
ASCALAR(airspeed_stall, "AIRSPEED_STALL", 0),
// @Param: FBWB_ELEV_REV
// @DisplayName: Fly By Wire elevator reverse
// @Description: Reverse sense of elevator in FBWB and CRUISE modes. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude.
@ -758,12 +782,13 @@ const AP_Param::Info Plane::var_info[] = {
GOBJECT(relay, "RELAY", AP_Relay),
#endif
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
// @Group: CHUTE_
// @Path: ../libraries/AP_Parachute/AP_Parachute.cpp
GOBJECT(parachute, "CHUTE_", AP_Parachute),
#endif
#if AP_RANGEFINDER_ENABLED
// @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
GOBJECT(rangefinder, "RNGFND", RangeFinder),
@ -774,6 +799,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Values: 0:Disabled,1:Enabled
// @User: Standard
GSCALAR(rangefinder_landing, "RNGFND_LANDING", 0),
#endif
#if AP_TERRAIN_AVAILABLE
// @Group: TERRAIN_
@ -965,9 +991,11 @@ const AP_Param::Info Plane::var_info[] = {
GOBJECT(rpm_sensor, "RPM", AP_RPM),
#endif
#if AP_RSSI_ENABLED
// @Group: RSSI_
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
GOBJECT(rssi, "RSSI_", AP_RSSI),
#endif
// @Group: NTF_
// @Path: ../libraries/AP_Notify/AP_Notify.cpp
@ -1084,6 +1112,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode
// @Bitmask: 12: Enable FBWB style loiter altitude control
// @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces
// @Bitmask: 14: In AUTO - climb to next waypoint altitude immediately instead of linear climb
// @User: Advanced
AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
@ -1189,11 +1218,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard
AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// @Group: GUIDED_
// @Path: ../libraries/AC_PID/AC_PID.cpp
AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID),
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// @Param: MAN_EXPO_ROLL
// @DisplayName: Manual control expo for roll

5
ArduPlane/Parameters.h
View File

@ -343,6 +343,7 @@ public:
k_param_mixing_offset,
k_param_dspoiler_rud_rate,
k_param_airspeed_stall,
k_param_logger = 253, // Logging Group
@ -356,6 +357,8 @@ public:
k_param_acro_yaw_rate,
k_param_takeoff_throttle_max_t,
k_param_autotune_options,
k_param_takeoff_throttle_min,
k_param_takeoff_options,
};
AP_Int16 format_version;
@ -552,7 +555,7 @@ public:
} fwd_batt_cmp;
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// guided yaw heading PID
AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.0};
#endif

42
ArduPlane/Plane.h
View File

@ -39,7 +39,7 @@
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_AHRS/AP_AHRS.h> // ArduPilot Mega DCM Library
#include <SRV_Channel/SRV_Channel.h>
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
#include <AP_RangeFinder/AP_RangeFinder_config.h> // Range finder library
#include <Filter/Filter.h> // Filter library
#include <AP_Camera/AP_Camera.h> // Photo or video camera
#include <AP_Terrain/AP_Terrain.h>
@ -206,7 +206,23 @@ private:
AP_Int8 *flight_modes = &g.flight_mode1;
const uint8_t num_flight_modes = 6;
#if AP_RANGEFINDER_ENABLED
AP_FixedWing::Rangefinder_State rangefinder_state;
#endif
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
struct {
// allow for external height above ground estimate
float hagl;
uint32_t last_update_ms;
uint32_t timeout_ms;
} external_hagl;
bool get_external_HAGL(float &height_agl);
void handle_external_hagl(const mavlink_command_int_t &packet);
#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
float get_landing_height(bool &using_rangefinder);
#if AP_RPM_ENABLED
AP_RPM rpm_sensor;
@ -350,7 +366,11 @@ private:
uint32_t AFS_last_valid_rc_ms;
} failsafe;
enum Landing_ApproachStage {
#if HAL_QUADPLANE_ENABLED
// Landing
class VTOLApproach {
public:
enum class Stage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
@ -359,10 +379,7 @@ private:
VTOL_LANDING,
};
#if HAL_QUADPLANE_ENABLED
// Landing
struct {
enum Landing_ApproachStage approach_stage;
Stage approach_stage;
float approach_direction_deg;
} vtol_approach_s;
#endif
@ -537,7 +554,7 @@ private:
float forced_throttle;
uint32_t last_forced_throttle_ms;
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// airspeed adjustments
float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed.
float target_airspeed_accel;
@ -556,7 +573,7 @@ private:
uint32_t target_heading_time_ms;
guided_heading_type_t target_heading_type;
bool target_heading_limit;
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
} guided_state;
#if AP_LANDINGGEAR_ENABLED
@ -633,7 +650,7 @@ private:
FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)};
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
AP_Parachute parachute;
#endif
@ -865,9 +882,11 @@ private:
float mission_alt_offset(void);
float height_above_target(void);
float lookahead_adjustment(void);
#if AP_RANGEFINDER_ENABLED
float rangefinder_correction(void);
void rangefinder_height_update(void);
void rangefinder_terrain_correction(float &height);
#endif
void stabilize();
void calc_throttle();
void calc_nav_roll();
@ -1074,8 +1093,10 @@ private:
bool rc_throttle_value_ok(void) const;
bool rc_failsafe_active(void) const;
#if AP_RANGEFINDER_ENABLED
// sensors.cpp
void read_rangefinder(void);
#endif
// system.cpp
void init_ardupilot() override;
@ -1094,6 +1115,7 @@ private:
bool auto_takeoff_check(void);
void takeoff_calc_roll(void);
void takeoff_calc_pitch(void);
void takeoff_calc_throttle(const bool use_max_throttle=false);
int8_t takeoff_tail_hold(void);
int16_t get_takeoff_pitch_min_cd(void);
void landing_gear_update(void);
@ -1134,7 +1156,7 @@ private:
// parachute.cpp
void parachute_check();
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
void do_parachute(const AP_Mission::Mission_Command& cmd);
void parachute_release();
bool parachute_manual_release();

23
ArduPlane/RC_Channel.cpp
View File

@ -95,9 +95,9 @@ void RC_Channel_Plane::do_aux_function_crow_mode(AuxSwitchPos ch_flag)
}
}
#if HAL_SOARING_ENABLED
void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag)
{
#if HAL_SOARING_ENABLED
SoaringController::ActiveStatus desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;
switch (ch_flag) {
@ -113,8 +113,8 @@ void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag)
}
plane.g2.soaring_controller.set_pilot_desired_state(desired_state);
#endif
}
#endif
void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
{
@ -169,6 +169,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
case AUX_FUNC::FW_AUTOTUNE:
case AUX_FUNC::VFWD_THR_OVERRIDE:
case AUX_FUNC::PRECISION_LOITER:
#if QAUTOTUNE_ENABLED
case AUX_FUNC::AUTOTUNE_TEST_GAINS:
#endif
break;
case AUX_FUNC::SOARING:
@ -275,9 +278,11 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
}
#endif
#if HAL_SOARING_ENABLED
case AUX_FUNC::SOARING:
do_aux_function_soaring_3pos(ch_flag);
break;
#endif
case AUX_FUNC::FLAP:
case AUX_FUNC::FBWA_TAILDRAGGER:
@ -333,8 +338,8 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
break;
#endif
case AUX_FUNC::ARSPD_CALIBRATE:
#if AP_AIRSPEED_AUTOCAL_ENABLE
case AUX_FUNC::ARSPD_CALIBRATE:
switch (ch_flag) {
case AuxSwitchPos::HIGH:
plane.airspeed.set_calibration_enabled(true);
@ -345,20 +350,20 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
plane.airspeed.set_calibration_enabled(false);
break;
}
#endif
break;
#endif
case AUX_FUNC::LANDING_FLARE:
do_aux_function_flare(ch_flag);
break;
#if HAL_PARACHUTE_ENABLED
case AUX_FUNC::PARACHUTE_RELEASE:
#if PARACHUTE == ENABLED
if (ch_flag == AuxSwitchPos::HIGH) {
plane.parachute_manual_release();
}
#endif
break;
#endif
case AUX_FUNC::MODE_SWITCH_RESET:
rc().reset_mode_switch();
@ -439,6 +444,12 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
// handled by lua scripting, just ignore here
break;
#if QAUTOTUNE_ENABLED
case AUX_FUNC::AUTOTUNE_TEST_GAINS:
plane.quadplane.qautotune.do_aux_function(ch_flag);
break;
#endif
default:
return RC_Channel::do_aux_function(ch_option, ch_flag);
}

61
ArduPlane/ReleaseNotes.txt
View File

@ -1,5 +1,64 @@
ArduPilot Plane Release Notes:
------------------------------
------------------------------------------------------------------
Release 4.5.5 1st Aug 2024
No changes from 4.5.5-beta2
------------------------------------------------------------------
Release 4.5.5-beta2 27 July 2024
Changes from 4.5.5-beta1
1) Board specific enhancements and bug fixes
- CubeRed's second core disabled at boot to avoid spurious writes to RAM
- CubeRed bootloader's dual endpoint update method fixed
------------------------------------------------------------------
Release 4.5.5-beta1 1st July 2024
Changes from 4.5.4
1) Board specific enhancements and bug fixes
- fixed IOMCU transmission errors when using bdshot
- update relay parameter names on various boards
- add ASP5033 airspeed in minimal builds
- added RadiolinkPIX6
- fix Aocoda-RC H743Dual motor issue
- use ICM45686 as an ICM20649 alternative in CubeRedPrimary
2) System level minor enhancements and bug fixes
- correct use-after-free in script statistics
- added arming check for eeprom full
- fixed a block logging issue which caused log messages to be dropped
- enable Socket SO_REUSEADDR on LwIP
- removed IST8310 overrun message
- added Siyi ZT6 support
- added BTFL sidebar symbols to the OSD
- added CRSF extended link stats to the OSD
- use the ESC with the highest RPM in the OSD when only one can be displayed
- support all Tramp power levels on high power VTXs
- emit jump count in missions even if no limit
- improve the bitmask indicating persistent parameters on bootloader flash
- fix duplicate error condition in the MicroStrain7
3) AHRS / EKF fixes
- fix infinite climb bug when using EK3_OGN_HGT_MASK
4) Plane specific changes
- fix rangefinder correction when terrain following is off
- correct description of MIN_GROUNDSPEED parameter
- correct Q_TRIM_PITCH description
- ensure the dshot type gets set at startup
5) Other minor enhancements and bug fixes
- specify pymonocypher version in more places
- added DroneCAN dependencies to custom builds
------------------------------------------------------------------
Release 4.5.4 12th June 2024
Changes from 4.5.3

93
ArduPlane/altitude.cpp
View File

@ -81,6 +81,13 @@ void Plane::setup_glide_slope(void)
break;
case Mode::Number::AUTO:
//climb without doing glide slope if option is enabled
if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) {
reset_offset_altitude();
break;
}
// we only do glide slide handling in AUTO when above 20m or
// when descending. The 20 meter threshold is arbitrary, and
// is basically to prevent situations where we try to slowly
@ -115,11 +122,21 @@ int32_t Plane::get_RTL_altitude_cm() const
*/
float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available)
{
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
float height_AGL;
// use external HAGL if available
if (get_external_HAGL(height_AGL)) {
return height_AGL;
}
#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
#if AP_RANGEFINDER_ENABLED
if (use_rangefinder_if_available && rangefinder_state.in_range) {
return rangefinder_state.height_estimate;
}
#endif
#if HAL_QUADPLANE_ENABLED
#if HAL_QUADPLANE_ENABLED && AP_RANGEFINDER_ENABLED
if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) {
// a special case for quadplane landing when rangefinder goes
@ -252,8 +269,10 @@ int32_t Plane::relative_target_altitude_cm(void)
target_altitude.lookahead = lookahead_adjustment();
relative_home_height += target_altitude.lookahead;
#if AP_RANGEFINDER_ENABLED
// correct for rangefinder data
relative_home_height += rangefinder_correction();
#endif
// we are following terrain, and have terrain data for the
// current location. Use it.
@ -262,7 +281,9 @@ int32_t Plane::relative_target_altitude_cm(void)
#endif
int32_t relative_alt = target_altitude.amsl_cm - home.alt;
relative_alt += mission_alt_offset()*100;
#if AP_RANGEFINDER_ENABLED
relative_alt += rangefinder_correction() * 100;
#endif
return relative_alt;
}
@ -499,9 +520,9 @@ int32_t Plane::adjusted_altitude_cm(void)
}
/*
return home-relative altitude adjusted for ALT_OFFSET This is useful
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
or to adjust the flying height of a long mission.
*/
int32_t Plane::adjusted_relative_altitude_cm(void)
{
@ -608,6 +629,7 @@ float Plane::lookahead_adjustment(void)
}
#if AP_RANGEFINDER_ENABLED
/*
correct target altitude using rangefinder data. Returns offset in
meters to correct target altitude. A positive number means we need
@ -747,6 +769,7 @@ void Plane::rangefinder_height_update(void)
}
}
#endif // AP_RANGEFINDER_ENABLED
/*
determine if Non Auto Terrain Disable is active and allowed in present control mode
@ -802,3 +825,67 @@ bool Plane::terrain_enabled_in_mode(Mode::Number num) const
return false;
}
#endif
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
/*
handle a MAV_CMD_SET_HAGL request. The accuracy is ignored
*/
void Plane::handle_external_hagl(const mavlink_command_int_t &packet)
{
auto &hagl = plane.external_hagl;
hagl.hagl = packet.param1;
hagl.last_update_ms = AP_HAL::millis();
hagl.timeout_ms = uint32_t(packet.param3 * 1000);
}
/*
get HAGL from external source if current
*/
bool Plane::get_external_HAGL(float &height_agl)
{
auto &hagl = plane.external_hagl;
if (hagl.last_update_ms != 0) {
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - hagl.last_update_ms <= hagl.timeout_ms) {
height_agl = hagl.hagl;
return true;
}
hagl.last_update_ms = 0;
}
return false;
}
#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
/*
get height for landing. Set using_rangefinder to true if a rangefinder
or external HAGL source is active
*/
float Plane::get_landing_height(bool &rangefinder_active)
{
float height;
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
// if external HAGL is active use that
if (get_external_HAGL(height)) {
// ensure no terrain correction is applied - that is the job
// of the external system if it is wanted
auto_state.terrain_correction = 0;
// an external HAGL is considered to be a type of rangefinder
rangefinder_active = true;
return height;
}
#endif
// get basic height above target
height = height_above_target();
rangefinder_active = false;
#if AP_RANGEFINDER_ENABLED
// possibly correct with rangefinder
height -= rangefinder_correction();
rangefinder_active = g.rangefinder_landing && rangefinder_state.in_range;
#endif
return height;
}

53
ArduPlane/commands_logic.cpp
View File

@ -144,21 +144,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_DO_LAND_START:
break;
case MAV_CMD_DO_FENCE_ENABLE:
#if AP_FENCE_ENABLED
if (cmd.p1 == 0) { // disable fence
plane.fence.enable(false);
gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled");
} else if (cmd.p1 == 1) { // enable fence
plane.fence.enable(true);
gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled");
} else if (cmd.p1 == 2) { // disable fence floor only
plane.fence.disable_floor();
gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled");
}
#endif
break;
case MAV_CMD_DO_AUTOTUNE_ENABLE:
autotune_enable(cmd.p1);
break;
@ -255,14 +240,16 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
} else {
// use rangefinder to correct if possible
float height = height_above_target() - rangefinder_correction();
bool rangefinder_active = false;
float height = plane.get_landing_height(rangefinder_active);
// for flare calculations we don't want to use the terrain
// correction as otherwise we will flare early on rising
// ground
height -= auto_state.terrain_correction;
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(),
g.rangefinder_landing && rangefinder_state.in_range);
rangefinder_active);
}
case MAV_CMD_NAV_LOITER_UNLIM:
@ -389,7 +376,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
next_WP_loc.lat = home.lat + 10;
next_WP_loc.lng = home.lng + 10;
auto_state.takeoff_speed_time_ms = 0;
auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction.
auto_state.height_below_takeoff_to_level_off_cm = 0;
// Flag also used to override "on the ground" throttle disable
@ -421,8 +408,10 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
auto_state.takeoff_pitch_cd = 1000;
}
#if AP_RANGEFINDER_ENABLED
// zero rangefinder state, start to accumulate good samples now
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
#endif
landing.do_land(cmd, relative_altitude);
@ -430,10 +419,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
// if we were in an abort we need to explicitly move out of the abort state, as it's sticky
set_flight_stage(AP_FixedWing::FlightStage::LAND);
}
#if AP_FENCE_ENABLED
plane.fence.auto_disable_fence_for_landing();
#endif
}
#if HAL_QUADPLANE_ENABLED
@ -444,7 +429,7 @@ void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
loc.sanitize(current_loc);
set_next_WP(loc);
vtol_approach_s.approach_stage = LOITER_TO_ALT;
vtol_approach_s.approach_stage = VTOLApproach::Stage::LOITER_TO_ALT;
}
#endif
@ -1031,7 +1016,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
loiter.direction = direction;
switch (vtol_approach_s.approach_stage) {
case RTL:
case VTOLApproach::Stage::RTL:
{
// fly home and loiter at RTL alt
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
@ -1039,11 +1024,11 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
// decend to Q RTL alt
plane.do_RTL(plane.home.alt + plane.quadplane.qrtl_alt*100UL);
plane.loiter_angle_reset();
vtol_approach_s.approach_stage = LOITER_TO_ALT;
vtol_approach_s.approach_stage = VTOLApproach::Stage::LOITER_TO_ALT;
}
break;
}
case LOITER_TO_ALT:
case VTOLApproach::Stage::LOITER_TO_ALT:
{
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
@ -1051,11 +1036,11 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
Vector3f wind = ahrs.wind_estimate();
vtol_approach_s.approach_direction_deg = degrees(atan2f(-wind.y, -wind.x));
gcs().send_text(MAV_SEVERITY_INFO, "Selected an approach path of %.1f", (double)vtol_approach_s.approach_direction_deg);
vtol_approach_s.approach_stage = ENSURE_RADIUS;
vtol_approach_s.approach_stage = VTOLApproach::Stage::ENSURE_RADIUS;
}
break;
}
case ENSURE_RADIUS:
case VTOLApproach::Stage::ENSURE_RADIUS:
{
// validate that the vehicle is at least the expected distance away from the loiter point
// require an angle total of at least 2 centidegrees, due to special casing of 1 centidegree
@ -1065,10 +1050,10 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
break;
}
vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT;
vtol_approach_s.approach_stage = VTOLApproach::Stage::WAIT_FOR_BREAKOUT;
FALLTHROUGH;
}
case WAIT_FOR_BREAKOUT:
case VTOLApproach::Stage::WAIT_FOR_BREAKOUT:
{
nav_controller->update_loiter(cmd.content.location, radius, direction);
@ -1077,7 +1062,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
// breakout when within 5 degrees of the opposite direction
if (fabsF(wrap_PI(ahrs.get_yaw() - breakout_direction_rad)) < radians(5.0f)) {
gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");
vtol_approach_s.approach_stage = APPROACH_LINE;
vtol_approach_s.approach_stage = VTOLApproach::Stage::APPROACH_LINE;
set_next_WP(cmd.content.location);
// fallthrough
} else {
@ -1085,7 +1070,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
FALLTHROUGH;
}
case APPROACH_LINE:
case VTOLApproach::Stage::APPROACH_LINE:
{
// project an apporach path
Location start = cmd.content.location;
@ -1114,7 +1099,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
if (past_finish_line && (lined_up || half_radius)) {
vtol_approach_s.approach_stage = VTOL_LANDING;
vtol_approach_s.approach_stage = VTOLApproach::Stage::VTOL_LANDING;
quadplane.do_vtol_land(cmd);
// fallthrough
} else {
@ -1122,7 +1107,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
FALLTHROUGH;
}
case VTOL_LANDING:
case VTOLApproach::Stage::VTOL_LANDING:
// nothing to do here, we should be into the quadplane landing code
return true;
}

18
ArduPlane/config.h
View File

@ -2,14 +2,6 @@
#include "defines.h"
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
// this avoids a very common config error
#define ENABLE ENABLED
#define DISABLE DISABLED
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
@ -213,14 +205,8 @@
# define FENCE_TRIGGERED_PIN -1
#endif
//////////////////////////////////////////////////////////////////////////////
// Parachute release
#ifndef PARACHUTE
#define PARACHUTE HAL_PARACHUTE_ENABLED
#endif
#ifndef OFFBOARD_GUIDED
#define OFFBOARD_GUIDED 1
#ifndef AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
#define AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED 1
#endif
//////////////////////////////////////////////////////////////////////////////

1
ArduPlane/defines.h
View File

@ -168,6 +168,7 @@ enum FlightOptions {
DISABLE_GROUND_PID_SUPPRESSION = (1<<11),
ENABLE_LOITER_ALT_CONTROL = (1<<12),
INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13),
IMMEDIATE_CLIMB_IN_AUTO = (1<<14),
};
enum CrowFlapOptions {

6
ArduPlane/events.cpp
View File

@ -138,7 +138,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
break;
}
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
@ -187,7 +187,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::GUIDED:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
@ -311,7 +311,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
break;
case Failsafe_Action_Parachute:
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
parachute_release();
#endif
break;

32
ArduPlane/fence.cpp
View File

@ -8,9 +8,32 @@
void Plane::fence_check()
{
const uint8_t orig_breaches = fence.get_breaches();
const bool armed = arming.is_armed();
uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
bool landing_or_landed = plane.flight_stage == AP_FixedWing::FlightStage::LAND
|| !armed
#if HAL_QUADPLANE_ENABLED
|| control_mode->mode_number() == Mode::Number::QLAND
|| quadplane.in_vtol_land_descent()
#endif
|| (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING);
// check for new breaches; new_breaches is bitmask of fence types breached
const uint8_t new_breaches = fence.check();
const uint8_t new_breaches = fence.check(landing_or_landed);
/*
if we are either disarmed or we are currently not in breach and
we are not flying then clear the state associated with the
previous mode breach handling. This allows the fence state
machine to reset at the end of a fence breach action such as an
RTL and autoland
*/
if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) {
if (!armed || ((new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) {
plane.previous_mode_reason = ModeReason::UNKNOWN;
}
}
if (!fence.enabled()) {
// Switch back to the chosen control mode if still in
@ -34,7 +57,7 @@ void Plane::fence_check()
// we still don't do anything when disarmed, but we do check for fence breaches.
// fence pre-arm check actually checks if any fence has been breached
// that's not ever going to be true if we don't call check on AP_Fence while disarmed
if (!arming.is_armed()) {
if (!armed) {
return;
}
@ -50,7 +73,7 @@ void Plane::fence_check()
}
if (new_breaches) {
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached");
fence.print_fence_message("breached", new_breaches);
// if the user wants some kind of response and motors are armed
const uint8_t fence_act = fence.get_action();
@ -115,7 +138,8 @@ void Plane::fence_check()
}
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
} else if (orig_breaches) {
} else if (orig_breaches && fence.get_breaches() == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
// record clearing of breach
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
}

2
ArduPlane/is_flying.cpp
View File

@ -162,7 +162,7 @@ void Plane::update_is_flying_5Hz(void)
#if HAL_ADSB_ENABLED
adsb.set_is_flying(new_is_flying);
#endif
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
parachute.set_is_flying(new_is_flying);
#endif
#if AP_STATS_ENABLED

4
ArduPlane/mode.cpp
View File

@ -54,7 +54,7 @@ bool Mode::enter()
plane.guided_state.last_forced_rpy_ms.zero();
plane.guided_state.last_forced_throttle_ms = 0;
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
@ -168,7 +168,9 @@ void Mode::update_target_altitude()
plane.set_target_altitude_location(plane.next_WP_loc);
} else if (plane.landing.is_on_approach()) {
plane.landing.setup_landing_glide_slope(plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.target_altitude.offset_cm);
#if AP_RANGEFINDER_ENABLED
plane.landing.adjust_landing_slope_for_rangefinder_bump(plane.rangefinder_state, plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.auto_state.wp_distance, plane.target_altitude.offset_cm);
#endif
} else if (plane.landing.get_target_altitude_location(target_location)) {
plane.set_target_altitude_location(target_location);
#if HAL_SOARING_ENABLED

6
ArduPlane/mode.h
View File

@ -816,6 +816,12 @@ protected:
Location start_loc;
bool _enter() override;
private:
// flag that we have already called autoenable fences once in MODE TAKEOFF
bool have_autoenabled_fences;
};
#if HAL_SOARING_ENABLED

8
ArduPlane/mode_guided.cpp
View File

@ -42,7 +42,7 @@ void ModeGuided::update()
plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
plane.update_load_factor();
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
// guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 )
// This function is used in Guided and AvoidADSB, check for guided
} else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
@ -70,7 +70,7 @@ void ModeGuided::update()
plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
plane.update_load_factor();
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
} else {
plane.calc_nav_roll();
}
@ -128,7 +128,7 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi
void ModeGuided::update_target_altitude()
{
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (((plane.guided_state.target_alt_time_ms != 0) || plane.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();
@ -148,7 +148,7 @@ void ModeGuided::update_target_altitude()
plane.guided_state.last_target_alt = temp.alt;
plane.set_target_altitude_location(temp);
} else
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
{
Mode::update_target_altitude();
}

3
ArduPlane/mode_qland.cpp
View File

@ -16,9 +16,6 @@ bool ModeQLand::_enter()
#if AP_LANDINGGEAR_ENABLED
plane.g2.landing_gear.deploy_for_landing();
#endif
#if AP_FENCE_ENABLED
plane.fence.auto_disable_fence_for_landing();
#endif
return true;
}

4
ArduPlane/mode_rtl.cpp
View File

@ -7,7 +7,7 @@ bool ModeRTL::_enter()
plane.do_RTL(plane.get_RTL_altitude_cm());
plane.rtl.done_climb = false;
#if HAL_QUADPLANE_ENABLED
plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
plane.vtol_approach_s.approach_stage = Plane::VTOLApproach::Stage::RTL;
// Quadplane specific checks
if (plane.quadplane.available()) {
@ -83,7 +83,7 @@ void ModeRTL::navigate()
AP_Mission::Mission_Command cmd;
cmd.content.location = plane.next_WP_loc;
plane.verify_landing_vtol_approach(cmd);
if (plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING) {
if (plane.vtol_approach_s.approach_stage == Plane::VTOLApproach::Stage::VTOL_LANDING) {
plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
}
return;

14
ArduPlane/mode_takeoff.cpp
View File

@ -22,11 +22,11 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = {
// @Increment: 1
// @Units: m
// @User: Standard
AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 5),
AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 10),
// @Param: LVL_PITCH
// @DisplayName: Takeoff mode altitude initial pitch
// @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT
// @Description: This is the target pitch during the takeoff.
// @Range: 0 30
// @Increment: 1
// @Units: deg
@ -63,6 +63,7 @@ ModeTakeoff::ModeTakeoff() :
bool ModeTakeoff::_enter()
{
takeoff_mode_setup = false;
have_autoenabled_fences = false;
return true;
}
@ -148,19 +149,22 @@ void ModeTakeoff::update()
if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
//below TAKOFF_LVL_ALT
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0);
plane.takeoff_calc_roll();
plane.takeoff_calc_pitch();
plane.takeoff_calc_throttle(true);
} else {
if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering
if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering
#if AP_FENCE_ENABLED
if (!have_autoenabled_fences) {
plane.fence.auto_enable_fence_after_takeoff();
have_autoenabled_fences = true;
}
#endif
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
} else { // still climbing to TAKEOFF_ALT; may be loitering
plane.calc_throttle();
plane.takeoff_calc_throttle();
plane.takeoff_calc_roll();
plane.takeoff_calc_pitch();
}

10
ArduPlane/navigation.cpp
View File

@ -116,8 +116,8 @@ float Plane::mode_auto_target_airspeed_cm()
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.landing_with_fixed_wing_spiral_approach() &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
((vtol_approach_s.approach_stage == VTOLApproach::Stage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == VTOLApproach::Stage::VTOL_LANDING))) {
const float land_airspeed = TECS_controller.get_land_airspeed();
if (is_positive(land_airspeed)) {
return land_airspeed * 100;
@ -187,7 +187,7 @@ void Plane::calc_airspeed_errors()
target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) *
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
}
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
} else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped
// offboard airspeed demanded
uint32_t now = AP_HAL::millis();
@ -203,7 +203,7 @@ void Plane::calc_airspeed_errors()
target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
}
#endif // OFFBOARD_GUIDED == ENABLED
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
#if HAL_SOARING_ENABLED
} else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
@ -256,7 +256,7 @@ void Plane::calc_airspeed_errors()
}
// when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
#if OFFBOARD_GUIDED == ENABLED
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
}

4
ArduPlane/parachute.cpp
View File

@ -6,13 +6,13 @@
*/
void Plane::parachute_check()
{
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
parachute.update();
parachute.check_sink_rate();
#endif
}
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
/*
parachute_release - trigger the release of the parachute

43
ArduPlane/quadplane.cpp
View File

@ -150,7 +150,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
AP_GROUPINFO("TRAN_PIT_MAX", 29, QuadPlane, transition_pitch_max, 3),
// frame class was moved from 30 when consolidating AP_Motors classes
#define FRAME_CLASS_OLD_IDX 30
// @Param: FRAME_CLASS
// @DisplayName: Frame Class
// @Description: Controls major frame class for multicopter component
@ -279,7 +279,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Bitmask: 19: CompleteTransition-to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND
// @Bitmask: 20: Force RTL mode-forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL)
// @Bitmask: 21: Tilt rotor-tilt motors up when disarmed in FW modes (except manual) to prevent ground strikes.
// @Bitmask: 22: Scale FF by the ratio of VTOL/plane angle P gains in VTOL modes rather than reducing VTOL angle P based on airspeed.
// @Bitmask: 22: Scale FF by the ratio of VTOL to plane angle P gains in Position 1 phase of transition into VTOL flight as well as reducing VTOL angle P based on airspeed.
AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
@ -671,34 +671,6 @@ bool QuadPlane::setup(void)
return false;
}
/*
cope with upgrade from old AP_Motors values for frame_class
*/
AP_Int8 old_class;
const AP_Param::ConversionInfo cinfo { Parameters::k_param_quadplane, FRAME_CLASS_OLD_IDX, AP_PARAM_INT8, nullptr };
if (AP_Param::find_old_parameter(&cinfo, &old_class) && !frame_class.load()) {
uint8_t new_value = 0;
// map from old values to new values
switch (old_class.get()) {
case 0:
new_value = AP_Motors::MOTOR_FRAME_QUAD;
break;
case 1:
new_value = AP_Motors::MOTOR_FRAME_HEXA;
break;
case 2:
new_value = AP_Motors::MOTOR_FRAME_OCTA;
break;
case 3:
new_value = AP_Motors::MOTOR_FRAME_OCTAQUAD;
break;
case 4:
new_value = AP_Motors::MOTOR_FRAME_Y6;
break;
}
frame_class.set_and_save(new_value);
}
if (hal.util->available_memory() <
4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav) + sizeof(*ahrs_view) + sizeof(*loiter_nav) + sizeof(*weathervane)) {
AP_BoardConfig::config_error("Not enough memory for quadplane");
@ -3127,8 +3099,6 @@ void QuadPlane::takeoff_controller(void)
if (no_navigation) {
pos_control->relax_velocity_controller_xy();
} else {
pos_control->set_accel_desired_xy_cmss(zero);
pos_control->set_vel_desired_xy_cms(vel);
pos_control->input_vel_accel_xy(vel, zero);
// nav roll and pitch are controller by position controller
@ -3554,9 +3524,6 @@ bool QuadPlane::verify_vtol_land(void)
poscontrol.pilot_correction_done = false;
pos_control->set_lean_angle_max_cd(0);
poscontrol.xy_correction.zero();
#if AP_FENCE_ENABLED
plane.fence.auto_disable_fence_for_landing();
#endif
#if AP_LANDINGGEAR_ENABLED
plane.g2.landing_gear.deploy_for_landing();
#endif
@ -3777,7 +3744,11 @@ float QuadPlane::forward_throttle_pct()
// approach the landing point when landing below the takeoff point
vel_forward.last_pct = vel_forward.integrator;
} else if ((in_vtol_land_final() && motors->limit.throttle_lower) ||
#if AP_RANGEFINDER_ENABLED
(plane.g.rangefinder_landing && (plane.rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow))) {
#else
false) {
#endif
// we're in the settling phase of landing or using a rangefinder that is out of range low, disable fwd motor
vel_forward.last_pct = 0;
vel_forward.integrator = 0;
@ -3981,7 +3952,7 @@ bool QuadPlane::is_vtol_land(uint16_t id) const
{
if (id == MAV_CMD_NAV_VTOL_LAND || id == MAV_CMD_NAV_PAYLOAD_PLACE) {
if (landing_with_fixed_wing_spiral_approach()) {
return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING;
return plane.vtol_approach_s.approach_stage == Plane::VTOLApproach::Stage::VTOL_LANDING;
} else {
return true;
}

3
ArduPlane/sensors.cpp
View File

@ -2,6 +2,7 @@
#include <AP_RSSI/AP_RSSI.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#if AP_RANGEFINDER_ENABLED
/*
read the rangefinder and update height estimate
*/
@ -33,3 +34,5 @@ void Plane::read_rangefinder(void)
rangefinder_height_update();
}
#endif // AP_RANGEFINDER_ENABLED

59
ArduPlane/servos.cpp
View File

@ -78,7 +78,7 @@ bool Plane::suppress_throttle(void)
return false;
}
#if PARACHUTE == ENABLED
#if HAL_PARACHUTE_ENABLED
if (control_mode->does_auto_throttle() && parachute.release_initiated()) {
// throttle always suppressed in auto-throttle modes after parachute release initiated
throttle_suppressed = true;
@ -499,47 +499,84 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
#endif // #if AP_BATTERY_WATT_MAX_ENABLED
/*
Apply min/max limits to throttle
Apply min/max safety limits to throttle.
*/
float Plane::apply_throttle_limits(float throttle_in)
{
// convert 0 to 100% (or -100 to +100) into PWM
// Pull the base throttle limits.
// These are usually set to map the ESC operating range.
int8_t min_throttle = aparm.throttle_min.get();
int8_t max_throttle = aparm.throttle_max.get();
#if AP_ICENGINE_ENABLED
// apply idle governor
// Apply idle governor.
g2.ice_control.update_idle_governor(min_throttle);
#endif
// If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0.
if (min_throttle < 0 && !allow_reverse_thrust()) {
// reverse thrust is available but inhibited.
min_throttle = 0;
}
const bool use_takeoff_throttle_max =
#if HAL_QUADPLANE_ENABLED
quadplane.in_transition() ||
#endif
// Query the conditions where TKOFF_THR_MAX applies.
const bool use_takeoff_throttle =
(flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
(flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
if (use_takeoff_throttle_max) {
// Handle throttle limits for takeoff conditions.
if (use_takeoff_throttle) {
if (aparm.takeoff_throttle_max != 0) {
// Replace max throttle with the takeoff max throttle setting.
// This is typically done to protect against long intervals of large power draw.
// Or (in contrast) to give some extra throttle during the initial climb.
max_throttle = aparm.takeoff_throttle_max.get();
}
// Do not allow min throttle to go below a lower threshold.
// This is typically done to protect against premature stalls close to the ground.
const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
if (!use_throttle_range || !ahrs.using_airspeed_sensor()) {
// Use a constant max throttle throughout the takeoff or when airspeed readings are not available.
if (aparm.takeoff_throttle_max.get() == 0) {
min_throttle = MAX(min_throttle, aparm.throttle_max.get());
} else {
min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get());
}
} else if (use_throttle_range) { // Use a throttle range through the takeoff.
if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1.
min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
}
}
} else if (landing.is_flaring()) {
// Allow throttle cutoff when flaring.
// This is to allow the aircraft to bleed speed faster and land with a shut off thruster.
min_throttle = 0;
}
// compensate for battery voltage drop
// Handle throttle limits for transition conditions.
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_transition()) {
if (aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max.get();
}
}
#endif
// Compensate the limits for battery voltage drop.
// This relaxes the limits when the battery is getting depleted.
g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle);
#if AP_BATTERY_WATT_MAX_ENABLED
// apply watt limiter
// Ensure that the power draw limits are not exceeded.
throttle_watt_limiter(min_throttle, max_throttle);
#endif
// Do a sanity check on them. Constrain down if necessary.
min_throttle = MIN(min_throttle, max_throttle);
// Let TECS know about the updated throttle limits.
TECS_controller.set_throttle_min(0.01f*min_throttle);
TECS_controller.set_throttle_max(0.01f*max_throttle);
return constrain_float(throttle_in, min_throttle, max_throttle);
}

6
ArduPlane/system.cpp
View File

@ -36,14 +36,18 @@ void Plane::init_ardupilot()
// init baro
barometer.init();
#if AP_RANGEFINDER_ENABLED
// initialise rangefinder
rangefinder.set_log_rfnd_bit(MASK_LOG_SONAR);
rangefinder.init(ROTATION_PITCH_270);
#endif
// initialise battery monitoring
battery.init();
#if AP_RSSI_ENABLED
rssi.init();
#endif
#if AP_RPM_ENABLED
rpm_sensor.init();
@ -53,7 +57,7 @@ void Plane::init_ardupilot()
gcs().setup_uarts();
#if OSD_ENABLED == ENABLED
#if OSD_ENABLED
osd.init();
#endif

25
ArduPlane/takeoff.cpp
View File

@ -219,7 +219,30 @@ void Plane::takeoff_calc_pitch(void)
}
/*
* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
* Set the throttle limits to run at during a takeoff.
*/
void Plane::takeoff_calc_throttle(const bool use_max_throttle) {
// This setting will take effect at the next run of TECS::update_pitch_throttle().
// Set the maximum throttle limit.
if (aparm.takeoff_throttle_max != 0) {
TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max);
}
// Set the minimum throttle limit.
const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit.
float min_throttle = (aparm.takeoff_throttle_max != 0) ? 0.01f*aparm.takeoff_throttle_max : 0.01f*aparm.throttle_max;
TECS_controller.set_throttle_min(min_throttle);
} else { // TKOFF_MODE == 1, allow for a throttle range.
if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN.
TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min);
}
}
calc_throttle();
}
/* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
*/
int16_t Plane::get_takeoff_pitch_min_cd(void)
{

8
ArduSub/AP_Arming_Sub.cpp
View File

@ -149,6 +149,14 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
// flag exiting this function
in_arm_motors = false;
// if we do not have an ekf origin then we can't use the WMM tables
if (!sub.ensure_ekf_origin()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Compass performance degraded");
if (check_enabled(ARMING_CHECK_PARAMETERS)) {
check_failed(ARMING_CHECK_PARAMETERS, true, "No world position, check ORIGIN_* parameters");
return false;
}
}
// return success
return true;
}

53
ArduSub/ArduSub.cpp
View File

@ -158,10 +158,7 @@ void Sub::fifty_hz_loop()
failsafe_sensors_check();
// Update rc input/output
rc().read_input();
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
}
// update_batt_compass - read battery and compass
@ -271,6 +268,9 @@ void Sub::three_hz_loop()
// one_hz_loop - runs at 1Hz
void Sub::one_hz_loop()
{
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
bool arm_check = arming.pre_arm_checks(false);
ap.pre_arm_check = arm_check;
AP_Notify::flags.pre_arm_check = arm_check;
@ -429,4 +429,51 @@ float Sub::get_alt_msl() const
return -posD;
}
bool Sub::ensure_ekf_origin()
{
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
// ekf origin is set
return true;
}
if (gps.num_sensors() > 0) {
// wait for the gps sensor to set the origin
// alert the pilot to poor compass performance
return false;
}
auto backup_origin = Location(static_cast<int32_t>(sub.g2.backup_origin_lat * 1e7),
static_cast<int32_t>(sub.g2.backup_origin_lon * 1e7),
static_cast<int32_t>(sub.g2.backup_origin_alt * 100),
Location::AltFrame::ABSOLUTE);
if (backup_origin.lat == 0 || backup_origin.lng == 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Backup location parameters are missing or zero");
return false;
}
if (!check_latlng(backup_origin.lat, backup_origin.lng)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Backup location parameters are not valid");
return false;
}
if (!ahrs.set_origin(backup_origin)) {
// a possible problem is that ek3_srcn_posxy is set to 3 (gps)
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to set origin, check EK3_SRC parameters");
return false;
}
gcs().send_text(MAV_SEVERITY_INFO, "Using backup location");
#if HAL_LOGGING_ENABLED
ahrs.Log_Write_Home_And_Origin();
#endif
// send ekf origin to GCS
gcs().send_message(MSG_ORIGIN);
return true;
}
AP_HAL_MAIN_CALLBACKS(&sub);

45
ArduSub/GCS_Mavlink.cpp
View File

@ -381,7 +381,9 @@ static const ap_message STREAM_POSITION_msgs[] = {
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
MSG_SERVO_OUTPUT_RAW,
MSG_RC_CHANNELS,
#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
#endif
};
static const ap_message STREAM_EXTRA1_msgs[] = {
MSG_ATTITUDE,
@ -483,6 +485,46 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc)
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
{
const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
if (!sub.flightmode->in_guided_mode() && !change_modes) {
return MAV_RESULT_DENIED;
}
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
return MAV_RESULT_DENIED;
}
Location request_location;
if (!location_from_command_t(packet, request_location)) {
return MAV_RESULT_DENIED;
}
if (request_location.sanitize(sub.current_loc)) {
// if the location wasn't already sane don't load it
return MAV_RESULT_DENIED; // failed as the location is not valid
}
// we need to do this first, as we don't want to change the flight mode unless we can also set the target
if (!sub.mode_guided.guided_set_destination(request_location)) {
return MAV_RESULT_FAILED;
}
if (!sub.flightmode->in_guided_mode()) {
if (!sub.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
// the position won't have been loaded if we had to change the flight mode, so load it again
if (!sub.mode_guided.guided_set_destination(request_location)) {
return MAV_RESULT_FAILED;
}
}
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
switch(packet.command) {
@ -496,6 +538,9 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_
case MAV_CMD_DO_MOTOR_TEST:
return handle_MAV_CMD_DO_MOTOR_TEST(packet);
case MAV_CMD_DO_REPOSITION:
return handle_command_int_do_reposition(packet);
case MAV_CMD_MISSION_START:
return handle_MAV_CMD_MISSION_START(packet);

1
ArduSub/GCS_Mavlink.h
View File

@ -22,6 +22,7 @@ protected:
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
// override sending of scaled_pressure3 to send on-board temperature:
void send_scaled_pressure3() override;

2
ArduSub/GCS_Sub.cpp
View File

@ -73,7 +73,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags()
}
#endif
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (sub.rangefinder_state.enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

31
ArduSub/Parameters.cpp
View File

@ -78,6 +78,14 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISARM),
// @Param: FS_GCS_TIMEOUT
// @DisplayName: GCS failsafe timeout
// @Description: Timeout before triggering the GCS failsafe
// @Units: s
// @Increment: 1
// @User: Standard
GSCALAR(failsafe_gcs_timeout, "FS_GCS_TIMEOUT", FS_GCS_TIMEOUT_S),
// @Param: FS_LEAK_ENABLE
// @DisplayName: Leak Failsafe Enable
// @Description: Controls what action to take if a leak is detected.
@ -628,7 +636,7 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
GOBJECT(mission, "MIS_", AP_Mission),
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
// @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
GOBJECT(rangefinder, "RNGFND", RangeFinder),
@ -706,7 +714,26 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// 18 was scripting
// 19 was airspeed
// @Param: ORIGIN_LAT
// @DisplayName: Backup latitude for EKF origin
// @Description: Backup EKF origin latitude used when not using a positioning system.
// @Units: deg
// @User: Standard
AP_GROUPINFO("ORIGIN_LAT", 19, ParametersG2, backup_origin_lat, 0),
// @Param: ORIGIN_LON
// @DisplayName: Backup longitude for EKF origin
// @Description: Backup EKF origin longitude used when not using a positioning system.
// @Units: deg
// @User: Standard
AP_GROUPINFO("ORIGIN_LON", 20, ParametersG2, backup_origin_lon, 0),
// @Param: ORIGIN_ALT
// @DisplayName: Backup altitude (MSL) for EKF origin
// @Description: Backup EKF origin altitude (MSL) used when not using a positioning system.
// @Units: m
// @User: Standard
AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0),
AP_GROUPEND
};

7
ArduSub/Parameters.h
View File

@ -185,6 +185,7 @@ public:
k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor
k_param_failsafe_pilot_input,
k_param_failsafe_pilot_input_timeout,
k_param_failsafe_gcs_timeout,
// Misc Sub settings
@ -243,7 +244,7 @@ public:
AP_Float throttle_filt;
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
AP_Int8 rangefinder_signal_min; // minimum signal quality for good rangefinder readings
AP_Float surftrak_depth; // surftrak will try to keep sub below this depth
#endif
@ -257,6 +258,7 @@ public:
AP_Int8 failsafe_terrain;
AP_Int8 failsafe_pilot_input; // pilot input failsafe behavior
AP_Float failsafe_pilot_input_timeout;
AP_Float failsafe_gcs_timeout; // ground station failsafe timeout (seconds)
AP_Int8 xtrack_angle_limit;
@ -366,6 +368,9 @@ public:
// control over servo output ranges
SRV_Channels servo_channels;
AP_Float backup_origin_lat;
AP_Float backup_origin_lon;
AP_Float backup_origin_alt;
};
extern const AP_Param::Info var_info[];

19
ArduSub/Sub.h
View File

@ -232,7 +232,18 @@ private:
} failsafe;
bool any_failsafe_triggered() const {
return (failsafe.pilot_input || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain);
return (
failsafe.pilot_input
|| battery.has_failsafed()
|| failsafe.gcs
|| failsafe.ekf
|| failsafe.terrain
|| failsafe.leak
|| failsafe.internal_pressure
|| failsafe.internal_temperature
|| failsafe.crash
|| failsafe.sensor_health
);
}
// sensor health for logging
@ -428,6 +439,8 @@ private:
float get_alt_rel() const WARN_IF_UNUSED;
float get_alt_msl() const WARN_IF_UNUSED;
void exit_mission();
void set_origin(const Location& loc);
bool ensure_ekf_origin();
bool verify_loiter_unlimited();
bool verify_loiter_time();
bool verify_wait_delay();
@ -614,10 +627,10 @@ public:
// For Lua scripting, so index is 1..4, not 0..3
uint8_t get_and_clear_button_count(uint8_t index);
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); }
bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); }
#endif // RANGEFINDER_ENABLED
#endif // AP_RANGEFINDER_ENABLED
#endif // AP_SCRIPTING_ENABLED
};

4
ArduSub/config.h
View File

@ -46,10 +46,6 @@
// Rangefinder
//
#ifndef RANGEFINDER_ENABLED
# define RANGEFINDER_ENABLED ENABLED
#endif
#ifndef RANGEFINDER_HEALTH_MAX
# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder
#endif

4
ArduSub/defines.h
View File

@ -79,8 +79,8 @@ enum LoggingParameters {
#ifndef FS_GCS
# define FS_GCS DISABLED
#endif
#ifndef FS_GCS_TIMEOUT_MS
# define FS_GCS_TIMEOUT_MS 2500 // gcs failsafe triggers after this number of milliseconds with no GCS heartbeat
#ifndef FS_GCS_TIMEOUT_S
# define FS_GCS_TIMEOUT_S 5.0 // gcs failsafe triggers after this number of seconds with no GCS heartbeat
#endif
// missing terrain data failsafe

3
ArduSub/failsafe.cpp
View File

@ -321,7 +321,8 @@ void Sub::failsafe_gcs_check()
uint32_t tnow = AP_HAL::millis();
// Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) {
const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g.failsafe_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
if (tnow - gcs_last_seen_ms < gcs_timeout_ms) {
// Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);

2
ArduSub/joystick.cpp
View File

@ -190,7 +190,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
case JSButton::button_function_t::k_mode_poshold:
set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND);
break;
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
case JSButton::button_function_t::k_mode_surftrak:
set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND);
break;

1
ArduSub/mode.h
View File

@ -320,6 +320,7 @@ public:
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
bool in_guided_mode() const override { return true; }
bool guided_limit_check();
void guided_limit_init_time_and_pos();
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);

11
ArduSub/mode_auto.cpp
View File

@ -426,6 +426,7 @@ void ModeAuto::set_auto_yaw_roi(const Location &roi_location)
// Return true if it is possible to recover from a rangefinder failure
bool ModeAuto::auto_terrain_recover_start()
{
#if AP_RANGEFINDER_ENABLED
// Check rangefinder status to see if recovery is possible
switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
@ -462,6 +463,9 @@ bool ModeAuto::auto_terrain_recover_start()
gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
return true;
#else
return false;
#endif
}
// Attempt recovery from terrain failsafe
@ -470,7 +474,6 @@ bool ModeAuto::auto_terrain_recover_start()
void ModeAuto::auto_terrain_recover_run()
{
float target_climb_rate = 0;
static uint32_t rangefinder_recovery_ms = 0;
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
@ -483,6 +486,8 @@ void ModeAuto::auto_terrain_recover_run()
return;
}
#if AP_RANGEFINDER_ENABLED
static uint32_t rangefinder_recovery_ms = 0;
switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::Status::OutOfRangeLow:
@ -529,6 +534,10 @@ void ModeAuto::auto_terrain_recover_run()
rangefinder_recovery_ms = 0;
return;
}
#else
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
sub.failsafe_terrain_act();
#endif
// exit on failure (timeout)
if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {

6
ArduSub/mode_surftrak.cpp
View File

@ -39,8 +39,10 @@ bool ModeSurftrak::init(bool ignore_checks)
if (!sub.rangefinder_alt_ok()) {
sub.gcs().send_text(MAV_SEVERITY_INFO, "waiting for a rangefinder reading");
#if AP_RANGEFINDER_ENABLED
} else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to hold range", sub.g.surftrak_depth * 0.01f);
#endif
}
return true;
@ -60,6 +62,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
{
bool success = false;
#if AP_RANGEFINDER_ENABLED
if (sub.control_mode != Number::SURFTRAK) {
sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set");
} else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
@ -84,6 +87,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
} else {
reset();
}
#endif
return success;
}
@ -141,6 +145,7 @@ void ModeSurftrak::control_range() {
*/
void ModeSurftrak::update_surface_offset()
{
#if AP_RANGEFINDER_ENABLED
if (sub.rangefinder_alt_ok()) {
// Get the latest terrain offset
float rangefinder_terrain_offset_cm = sub.rangefinder_state.rangefinder_terrain_offset_cm;
@ -162,4 +167,5 @@ void ModeSurftrak::update_surface_offset()
sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm);
}
}
#endif // AP_RANGEFINDER_ENABLED
}

4
ArduSub/motors.cpp
View File

@ -18,7 +18,11 @@ void Sub::motors_output()
verify_motor_test();
} else {
motors.set_interlock(true);
SRV_Channels::cork();
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
motors.output();
SRV_Channels::push();
}
}

4
ArduSub/sensors.cpp
View File

@ -17,7 +17,7 @@ void Sub::read_barometer()
void Sub::init_rangefinder()
{
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
rangefinder.init(ROTATION_PITCH_270);
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
@ -28,7 +28,7 @@ void Sub::init_rangefinder()
// return rangefinder altitude in centimeters
void Sub::read_rangefinder()
{
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
rangefinder.update();
// signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a

3
ArduSub/system.cpp
View File

@ -129,7 +129,7 @@ void Sub::init_ardupilot()
last_pilot_heading = ahrs.yaw_sensor;
// initialise rangefinder
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
init_rangefinder();
#endif
@ -166,6 +166,7 @@ void Sub::startup_INS_ground()
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AP_AHRS::VehicleClass::SUBMARINE);
ahrs.set_fly_forward(false);
// Warm up and calibrate gyro offsets
ins.init(scheduler.get_loop_rate_hz());

2
ArduSub/terrain.cpp
View File

@ -8,7 +8,7 @@ void Sub::terrain_update()
// tell the rangefinder our height, so it can go into power saving
// mode if available
#if RANGEFINDER_ENABLED == ENABLED
#if AP_RANGEFINDER_ENABLED
float height;
if (terrain.height_above_terrain(height, true)) {
rangefinder.set_estimated_terrain_height(height);

2
Blimp/Blimp.cpp
View File

@ -165,9 +165,11 @@ void Blimp::ten_hz_logging_loop()
}
if (should_log(MASK_LOG_RCIN)) {
logger.Write_RCIN();
#if AP_RSSI_ENABLED
if (rssi.enabled()) {
logger.Write_RSSI();
}
#endif
}
if (should_log(MASK_LOG_RCOUT)) {
logger.Write_RCOUT();

5
Blimp/GCS_Mavlink.cpp
View File

@ -40,6 +40,9 @@ MAV_STATE GCS_MAVLINK_Blimp::vehicle_system_status() const
if (blimp.ap.land_complete) {
return MAV_STATE_STANDBY;
}
if (!blimp.ap.initialised) {
return MAV_STATE_BOOT;
}
return MAV_STATE_ACTIVE;
}
@ -340,7 +343,9 @@ static const ap_message STREAM_POSITION_msgs[] = {
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
MSG_SERVO_OUTPUT_RAW,
MSG_RC_CHANNELS,
#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
#endif
};
static const ap_message STREAM_EXTRA1_msgs[] = {
MSG_ATTITUDE,

2
Blimp/Parameters.cpp
View File

@ -382,9 +382,11 @@ const AP_Param::Info Blimp::var_info[] = {
GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
#endif
#if AP_RSSI_ENABLED
// @Group: RSSI_
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
GOBJECT(rssi, "RSSI_", AP_RSSI),
#endif
// @Group: NTF_
// @Path: ../libraries/AP_Notify/AP_Notify.cpp

Some files were not shown because too many files have changed in this diff Show More