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

Merge branch 'master' into ardupilot-guru

This commit is contained in:
Kursat Aktas 2024-11-27 17:37:31 +03:00 committed by GitHub
parent 03109d0b32 2f9ff3ef73
commit 5e708f30df
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
97 changed files with 2645 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,8 +32,6 @@ jobs:
name: focal
- os: ubuntu
name: jammy
- os: ubuntu
name: mantic
- os: ubuntu
name: noble
- os: archlinux

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

@ -229,7 +229,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}

4
.github/workflows/test_sitl_copter.yml vendored
View File

@ -247,7 +247,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@ -343,7 +343,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}

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

@ -230,7 +230,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}

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

@ -244,7 +244,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}

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

@ -244,7 +244,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}

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

@ -230,7 +230,7 @@ jobs:
NOW=$(date -u +"%F-%T")
echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
- name: ccache cache files
uses: actions/cache/restore@v3
uses: actions/cache/restore@v4
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}

20
.github/workflows/test_size.yml vendored
View File

@ -268,7 +268,7 @@ jobs:
shell: bash
run: |
cd pr/
Tools/scripts/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
Tools/scripts/build_tests/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
- name: Feature compare with ${{ github.event.pull_request.base.ref }}
shell: bash
@ -301,8 +301,26 @@ jobs:
Tools/scripts/extract_features.py "$EF_BASE_BRANCH_BINARY" -nm "${BIN_PREFIX}nm" >features-base_branch.txt
Tools/scripts/extract_features.py "$EF_PR_BRANCH_BINARY" -nm "${BIN_PREFIX}nm" >features-pr.txt
diff -u features-base_branch.txt features-pr.txt || true
diff_output=$(diff -u features-base_branch.txt features-pr.txt || true)
echo "### Features Diff Output" >> $GITHUB_STEP_SUMMARY
if [ -n "$diff_output" ]; then
echo '```diff' >> $GITHUB_STEP_SUMMARY
echo "$diff_output" >> $GITHUB_STEP_SUMMARY
echo '```' >> $GITHUB_STEP_SUMMARY
else
echo "No differences found." >> $GITHUB_STEP_SUMMARY
fi
- name: Checksum compare with ${{ github.event.pull_request.base.ref }}
shell: bash
run: |
diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true
diff_output=$(diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true || true)
echo "### Checksum Diff Output" >> $GITHUB_STEP_SUMMARY
if [ -n "$diff_output" ]; then
echo '```diff' >> $GITHUB_STEP_SUMMARY
echo "$diff_output" >> $GITHUB_STEP_SUMMARY
echo '```' >> $GITHUB_STEP_SUMMARY
else
echo "No differences found." >> $GITHUB_STEP_SUMMARY
fi

60
ArduCopter/Copter.cpp
View File

@ -285,13 +285,9 @@ bool Copter::set_target_location(const Location& target_loc)
return mode_guided.set_destination(target_loc);
}
#endif //MODE_GUIDED_ENABLED
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED
#if MODE_GUIDED_ENABLED
// start takeoff to given altitude (for use by scripting)
bool Copter::start_takeoff(float alt)
bool Copter::start_takeoff(const float alt)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
@ -304,7 +300,11 @@ bool Copter::start_takeoff(float alt)
}
return false;
}
#endif //MODE_GUIDED_ENABLED
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED
#if MODE_GUIDED_ENABLED
// set target position (for use by scripting)
bool Copter::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)
{
@ -411,7 +411,55 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
mode_guided.set_angle(q, ang_vel_body, throttle, true);
return true;
}
#endif
// Register a custom mode with given number and names
AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
{
const Mode::Number number = (Mode::Number)num;
// See if this mode has been registered already, if it has return the state for it
// This allows scripting restarts
for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
if (mode_guided_custom[i] == nullptr) {
break;
}
if ((mode_guided_custom[i]->mode_number() == number) &&
(strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
(strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
return &mode_guided_custom[i]->state;
}
}
// Number already registered to existing mode
if (mode_from_mode_num(number) != nullptr) {
return nullptr;
}
// Find free slot
for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
if (mode_guided_custom[i] == nullptr) {
// Duplicate strings so were not pointing to unknown memory
const char* full_name_copy = strdup(full_name);
const char* short_name_copy = strndup(short_name, 4);
if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
}
if (mode_guided_custom[i] == nullptr) {
// Allocation failure
return nullptr;
}
// Registration sucsessful, notify the GCS that it should re-request the avalable modes
gcs().available_modes_changed();
return &mode_guided_custom[i]->state;
}
}
// No free slots
return nullptr;
}
#endif // MODE_GUIDED_ENABLED
#if MODE_CIRCLE_ENABLED
// circle mode controls

8
ArduCopter/Copter.h
View File

@ -668,12 +668,12 @@ private:
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if MODE_GUIDED_ENABLED
bool set_target_location(const Location& target_loc) override;
bool start_takeoff(const float alt) override;
#endif // MODE_GUIDED_ENABLED
#endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED
#if MODE_GUIDED_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;
@ -684,6 +684,8 @@ private:
bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
// Register a custom mode with given number and names
AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;
#endif
#if MODE_CIRCLE_ENABLED
bool get_circle_radius(float &radius_m) override;
@ -1029,6 +1031,10 @@ private:
#endif
#if MODE_GUIDED_ENABLED
ModeGuided mode_guided;
#if AP_SCRIPTING_ENABLED
// Custom modes registered at runtime
ModeGuidedCustom *mode_guided_custom[5];
#endif
#endif
ModeLand mode_land;
#if MODE_LOITER_ENABLED

34
ArduCopter/GCS_Mavlink.cpp
View File

@ -1742,7 +1742,16 @@ uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
#endif
};
const uint8_t mode_count = ARRAY_SIZE(modes);
const uint8_t base_mode_count = ARRAY_SIZE(modes);
uint8_t mode_count = base_mode_count;
#if AP_SCRIPTING_ENABLED
for (uint8_t i = 0; i < ARRAY_SIZE(copter.mode_guided_custom); i++) {
if (copter.mode_guided_custom[i] != nullptr) {
mode_count += 1;
}
}
#endif
// Convert to zero indexed
const uint8_t index_zero = index - 1;
@ -1752,8 +1761,27 @@ uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
}
// Ask the mode for its name and number
const char* name = modes[index_zero]->name();
uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number();
const char* name;
uint8_t mode_number;
if (index_zero < base_mode_count) {
name = modes[index_zero]->name();
mode_number = (uint8_t)modes[index_zero]->mode_number();
} else {
#if AP_SCRIPTING_ENABLED
const uint8_t custom_index = index_zero - base_mode_count;
if (copter.mode_guided_custom[custom_index] == nullptr) {
// Invalid index, should not happen
return mode_count;
}
name = copter.mode_guided_custom[custom_index]->name();
mode_number = (uint8_t)copter.mode_guided_custom[custom_index]->mode_number();
#else
// Should not endup here
return mode_count;
#endif
}
#if MODE_AUTO_ENABLED
// Auto RTL is odd

87
ArduCopter/mode.cpp
View File

@ -32,158 +32,141 @@ PayloadPlace Mode::payload_place;
// return the static controller object corresponding to supplied mode
Mode *Copter::mode_from_mode_num(const Mode::Number mode)
{
Mode *ret = nullptr;
switch (mode) {
#if MODE_ACRO_ENABLED
case Mode::Number::ACRO:
ret = &mode_acro;
break;
return &mode_acro;
#endif
case Mode::Number::STABILIZE:
ret = &mode_stabilize;
break;
return &mode_stabilize;
case Mode::Number::ALT_HOLD:
ret = &mode_althold;
break;
return &mode_althold;
#if MODE_AUTO_ENABLED
case Mode::Number::AUTO:
ret = &mode_auto;
break;
return &mode_auto;
#endif
#if MODE_CIRCLE_ENABLED
case Mode::Number::CIRCLE:
ret = &mode_circle;
break;
return &mode_circle;
#endif
#if MODE_LOITER_ENABLED
case Mode::Number::LOITER:
ret = &mode_loiter;
break;
return &mode_loiter;
#endif
#if MODE_GUIDED_ENABLED
case Mode::Number::GUIDED:
ret = &mode_guided;
break;
return &mode_guided;
#endif
case Mode::Number::LAND:
ret = &mode_land;
break;
return &mode_land;
#if MODE_RTL_ENABLED
case Mode::Number::RTL:
ret = &mode_rtl;
break;
return &mode_rtl;
#endif
#if MODE_DRIFT_ENABLED
case Mode::Number::DRIFT:
ret = &mode_drift;
break;
return &mode_drift;
#endif
#if MODE_SPORT_ENABLED
case Mode::Number::SPORT:
ret = &mode_sport;
break;
return &mode_sport;
#endif
#if MODE_FLIP_ENABLED
case Mode::Number::FLIP:
ret = &mode_flip;
break;
return &mode_flip;
#endif
#if AUTOTUNE_ENABLED
case Mode::Number::AUTOTUNE:
ret = &mode_autotune;
break;
return &mode_autotune;
#endif
#if MODE_POSHOLD_ENABLED
case Mode::Number::POSHOLD:
ret = &mode_poshold;
break;
return &mode_poshold;
#endif
#if MODE_BRAKE_ENABLED
case Mode::Number::BRAKE:
ret = &mode_brake;
break;
return &mode_brake;
#endif
#if MODE_THROW_ENABLED
case Mode::Number::THROW:
ret = &mode_throw;
break;
return &mode_throw;
#endif
#if HAL_ADSB_ENABLED
case Mode::Number::AVOID_ADSB:
ret = &mode_avoid_adsb;
break;
return &mode_avoid_adsb;
#endif
#if MODE_GUIDED_NOGPS_ENABLED
case Mode::Number::GUIDED_NOGPS:
ret = &mode_guided_nogps;
break;
return &mode_guided_nogps;
#endif
#if MODE_SMARTRTL_ENABLED
case Mode::Number::SMART_RTL:
ret = &mode_smartrtl;
break;
return &mode_smartrtl;
#endif
#if MODE_FLOWHOLD_ENABLED
case Mode::Number::FLOWHOLD:
ret = (Mode *)g2.mode_flowhold_ptr;
break;
return (Mode *)g2.mode_flowhold_ptr;
#endif
#if MODE_FOLLOW_ENABLED
case Mode::Number::FOLLOW:
ret = &mode_follow;
break;
return &mode_follow;
#endif
#if MODE_ZIGZAG_ENABLED
case Mode::Number::ZIGZAG:
ret = &mode_zigzag;
break;
return &mode_zigzag;
#endif
#if MODE_SYSTEMID_ENABLED
case Mode::Number::SYSTEMID:
ret = (Mode *)g2.mode_systemid_ptr;
break;
return (Mode *)g2.mode_systemid_ptr;
#endif
#if MODE_AUTOROTATE_ENABLED
case Mode::Number::AUTOROTATE:
ret = &mode_autorotate;
break;
return &mode_autorotate;
#endif
#if MODE_TURTLE_ENABLED
case Mode::Number::TURTLE:
ret = &mode_turtle;
break;
return &mode_turtle;
#endif
default:
break;
}
return ret;
#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
// Check registered custom modes
for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
if ((mode_guided_custom[i] != nullptr) && (mode_guided_custom[i]->mode_number() == mode)) {
return mode_guided_custom[i];
}
}
#endif
return nullptr;
}

31
ArduCopter/mode.h
View File

@ -1193,14 +1193,37 @@ private:
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
// controls which controller is run (pos or vel):
SubMode guided_mode = SubMode::TakeOff;
bool send_notification; // used to send one time notification to ground station
bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear)
static SubMode guided_mode;
static bool send_notification; // used to send one time notification to ground station
static bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear)
// guided mode is paused or not
bool _paused;
static bool _paused;
};
#if AP_SCRIPTING_ENABLED
// Mode which behaves as guided with custom mode number and name
class ModeGuidedCustom : public ModeGuided {
public:
// constructor registers custom number and names
ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name);
bool init(bool ignore_checks) override;
Number mode_number() const override { return number; }
const char *name() const override { return full_name; }
const char *name4() const override { return short_name; }
// State object which can be edited by scripting
AP_Vehicle::custom_mode_state state;
private:
const Number number;
const char* full_name;
const char* short_name;
};
#endif
class ModeGuidedNoGPS : public ModeGuided {

2
ArduCopter/mode_auto.cpp
View File

@ -824,7 +824,7 @@ void ModeAuto::exit_mission()
bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
{
// only process guided waypoint if we are in guided mode
if (copter.flightmode->mode_number() != Mode::Number::GUIDED && !(copter.flightmode->mode_number() == Mode::Number::AUTO && _mode == SubMode::NAVGUIDED)) {
if (!copter.flightmode->in_guided_mode()) {
return false;
}

12
ArduCopter/mode_guided.cpp
View File

@ -7,7 +7,7 @@
*/
static Vector3p guided_pos_target_cm; // position target (used by posvel controller only)
bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain
static bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain
static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller)
static Vector3f guided_accel_target_cmss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller
@ -30,7 +30,15 @@ struct Guided_Limit {
float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
uint32_t start_time;// system time in milliseconds that control was handed to the external computer
Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit
} guided_limit;
} static guided_limit;
// controls which controller is run (pos or vel):
ModeGuided::SubMode ModeGuided::guided_mode = SubMode::TakeOff;
bool ModeGuided::send_notification; // used to send one time notification to ground station
bool ModeGuided::takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear)
// guided mode is paused or not
bool ModeGuided::_paused;
// init - initialise guided controller
bool ModeGuided::init(bool ignore_checks)

23
ArduCopter/mode_guided_custom.cpp Normal file
View File

@ -0,0 +1,23 @@
#include "Copter.h"
#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
// constructor registers custom number and names
ModeGuidedCustom::ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name):
number(_number),
full_name(_full_name),
short_name(_short_name)
{
}
bool ModeGuidedCustom::init(bool ignore_checks)
{
// Stript can block entry
if (!state.allow_entry) {
return false;
}
// Guided entry checks must also pass
return ModeGuided::init(ignore_checks);
}
#endif

15
ArduPlane/ArduPlane.cpp
View File

@ -139,6 +139,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
#if AC_PRECLAND_ENABLED
SCHED_TASK(precland_update, 400, 50, 160),
#endif
#if AP_QUICKTUNE_ENABLED
SCHED_TASK(update_quicktune, 40, 100, 163),
#endif
};
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@ -1025,4 +1028,16 @@ void Plane::precland_update(void)
}
#endif
#if AP_QUICKTUNE_ENABLED
/*
update AP_Quicktune object. We pass the supports_quicktune() method
in so that quicktune can detect if the user changes to a
non-quicktune capable mode while tuning and the gains can be reverted
*/
void Plane::update_quicktune(void)
{
quicktune.update(control_mode->supports_quicktune());
}
#endif
AP_HAL_MAIN_CALLBACKS(&plane);

6
ArduPlane/Parameters.cpp
View File

@ -1038,6 +1038,12 @@ const AP_Param::Info Plane::var_info[] = {
// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
PARAM_VEHICLE_INFO,
#if AP_QUICKTUNE_ENABLED
// @Group: QWIK_
// @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp
GOBJECT(quicktune, "QWIK_", AP_Quicktune),
#endif
AP_VAREND
};

1
ArduPlane/Parameters.h
View File

@ -362,6 +362,7 @@ public:
k_param_takeoff_throttle_idle,
k_param_pullup = 270,
k_param_quicktune,
};
AP_Int16 format_version;

8
ArduPlane/Plane.h
View File

@ -330,6 +330,10 @@ private:
ModeThermal mode_thermal;
#endif
#if AP_QUICKTUNE_ENABLED
AP_Quicktune quicktune;
#endif
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO
Mode *control_mode = &mode_initializing;
@ -875,6 +879,10 @@ private:
static const TerrainLookupTable Terrain_lookup[];
#endif
#if AP_QUICKTUNE_ENABLED
void update_quicktune(void);
#endif
// Attitude.cpp
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);

10
ArduPlane/RC_Channel.cpp
View File

@ -1,6 +1,7 @@
#include "Plane.h"
#include "RC_Channel.h"
#include "qautotune.h"
// defining these two macros and including the RC_Channels_VarInfo
// header defines the parameter information common to all vehicle
@ -176,6 +177,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
#endif
#if QAUTOTUNE_ENABLED
case AUX_FUNC::AUTOTUNE_TEST_GAINS:
#endif
#if AP_QUICKTUNE_ENABLED
case AUX_FUNC::QUICKTUNE:
#endif
break;
@ -458,6 +462,12 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
break;
#endif
#if AP_QUICKTUNE_ENABLED
case AUX_FUNC::QUICKTUNE:
plane.quicktune.update_switch_pos(ch_flag);
break;
#endif
default:
return RC_Channel::do_aux_function(ch_option, ch_flag);
}

1
ArduPlane/RC_Channel.h
View File

@ -30,7 +30,6 @@ private:
void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag);
void do_aux_function_flare(AuxSwitchPos ch_flag);
};
class RC_Channels_Plane : public RC_Channels

2
ArduPlane/altitude.cpp
View File

@ -44,7 +44,7 @@ void Plane::check_home_alt_change(void)
next_WP_loc.alt += alt_change_cm;
}
// reset TECS to force the field elevation estimate to reset
TECS_controller.reset();
TECS_controller.offset_altitude(alt_change_cm * 0.01f);
}
auto_state.last_home_alt_cm = home_alt_cm;
}

21
ArduPlane/mode.h
View File

@ -11,6 +11,12 @@
#include <AP_Mission/AP_Mission.h>
#include "pullup.h"
#ifndef AP_QUICKTUNE_ENABLED
#define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
#endif
#include <AP_Quicktune/AP_Quicktune.h>
class AC_PosControl;
class AC_AttitudeControl_Multi;
class AC_Loiter;
@ -142,6 +148,11 @@ public:
// true if voltage correction should be applied to throttle
virtual bool use_battery_compensation() const;
#if AP_QUICKTUNE_ENABLED
// does this mode support VTOL quicktune?
virtual bool supports_quicktune() const { return false; }
#endif
protected:
// subclasses override this to perform checks before entering the mode
@ -325,6 +336,9 @@ protected:
bool _enter() override;
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
#if AP_QUICKTUNE_ENABLED
bool supports_quicktune() const override { return true; }
#endif
private:
float active_radius_m;
@ -662,6 +676,9 @@ public:
protected:
bool _enter() override;
#if AP_QUICKTUNE_ENABLED
bool supports_quicktune() const override { return true; }
#endif
};
class ModeQLoiter : public Mode
@ -688,6 +705,10 @@ protected:
bool _enter() override;
uint32_t last_target_loc_set_ms;
#if AP_QUICKTUNE_ENABLED
bool supports_quicktune() const override { return true; }
#endif
};
class ModeQLand : public Mode

2
ArduPlane/qautotune.h
View File

@ -8,7 +8,7 @@
#include "quadplane.h"
#ifndef QAUTOTUNE_ENABLED
#define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED
#define QAUTOTUNE_ENABLED (HAL_QUADPLANE_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#if QAUTOTUNE_ENABLED

1
ArduPlane/wscript
View File

@ -27,6 +27,7 @@ def build(bld):
'AP_Follow',
'AC_PrecLand',
'AP_IRLock',
'AP_Quicktune',
],
)

5
Tools/AP_Bootloader/board_types.txt
View File

@ -334,6 +334,8 @@ AP_HW_GEPRCF745BTHD 1501
AP_HW_HGLRCF405V4 1524
AP_HW_F4BY_F427 1530
AP_HW_MFT-SEMA100 2000
AP_HW_AET-H743-Basic 2024
@ -364,6 +366,9 @@ AP_HW_UAV-DEV-F9P-G4 5223
AP_HW_UAV-DEV-UM982-G4 5224
AP_HW_UAV-DEV-M20D-G4 5225
AP_HW_UAV-DEV-Sensorboard-G4 5226
AP_HW_UAV-DEV-PWM-G4 5227
AP_HW_UAV-DEV-AUAV-H7 5228
AP_HW_UAV-DEV-FC-H7 5229
# IDs 5240-5249 reserved for TM IT-Systemhaus
AP_HW_TM-SYS-BeastFC 5240

35
Tools/autotest/arduplane.py
View File

@ -5,6 +5,7 @@ AP_FLAKE8_CLEAN
'''
from __future__ import print_function
import copy
import math
import os
import signal
@ -6190,10 +6191,43 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
higher_home = home
higher_home.alt += 40
self.set_home(higher_home)
self.change_mode(mode)
self.wait_altitude(15, 25, relative=True, minimum_duration=10)
self.disarm_vehicle(force=True)
self.reboot_sitl()
def SetHomeAltChange2(self):
'''ensure TECS operates predictably as home altitude changes continuously'''
'''
This can happen when performing a ship landing, where the home
coordinates are continuously set by the ship GNSS RX.
'''
self.set_parameter('TRIM_THROTTLE', 70)
self.wait_ready_to_arm()
home = self.home_position_as_mav_location()
target_alt = 20
self.takeoff(target_alt, mode="TAKEOFF")
self.change_mode("LOITER")
self.delay_sim_time(20) # Let the plane settle.
tstart = self.get_sim_time()
test_time = 10 # Run the test for 10s.
pub_freq = 10
for i in range(test_time*pub_freq):
tnow = self.get_sim_time()
higher_home = copy.copy(home)
# Produce 1Hz sine waves in home altitude change.
higher_home.alt += 40*math.sin((tnow-tstart)*(2*math.pi))
self.set_home(higher_home)
if tnow-tstart > test_time:
break
self.delay_sim_time(1.0/pub_freq)
# Test if the altitude is still within bounds.
self.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=1, timeout=2)
self.disarm_vehicle(force=True)
self.reboot_sitl()
def ForceArm(self):
'''check force-arming functionality'''
self.set_parameter("SIM_GPS1_ENABLE", 0)
@ -6462,6 +6496,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.ScriptStats,
self.GPSPreArms,
self.SetHomeAltChange,
self.SetHomeAltChange2,
self.ForceArm,
self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
self.GliderPullup,

14
Tools/autotest/default_params/quadplane.parm
View File

@ -70,10 +70,16 @@ Q_M_PWM_MIN 1000
Q_M_PWM_MAX 2000
Q_M_BAT_VOLT_MAX 12.8
Q_M_BAT_VOLT_MIN 9.6
Q_A_RAT_RLL_P 0.15
Q_A_RAT_PIT_P 0.15
Q_A_RAT_RLL_D 0.002
Q_A_RAT_PIT_D 0.002
Q_A_RAT_RLL_P 0.108
Q_A_RAT_RLL_I 0.108
Q_A_RAT_RLL_D 0.001
Q_A_RAT_PIT_P 0.103
Q_A_RAT_PIT_I 0.103
Q_A_RAT_PIT_D 0.001
Q_A_RAT_YAW_P 0.2
Q_A_RAT_YAW_P 0.02
Q_A_ANG_RLL_P 6
Q_A_ANG_PIT_P 6
RTL_ALTITUDE 20.00
PTCH_RATE_FF 1.407055
RLL_RATE_FF 0.552741

2
Tools/autotest/logger_metadata/parse.py
View File

@ -182,7 +182,7 @@ class LoggerDocco(object):
return
# Make sure lengths match up
if len(fmts) != len(self.fields_order):
print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts} num_fields={len(self.fields_order)}")
print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts} num_fields={len(self.fields_order)} {self.fields_order}") # noqa:E501
return
# Loop through the list
for idx in range(0, len(fmts)):

90
Tools/autotest/quadplane.py
View File

@ -363,6 +363,20 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
def QAUTOTUNE(self):
'''test Plane QAutoTune mode'''
# adjust tune so QAUTOTUNE can cope
self.set_parameters({
"Q_A_RAT_RLL_P" : 0.15,
"Q_A_RAT_RLL_I" : 0.25,
"Q_A_RAT_RLL_D" : 0.002,
"Q_A_RAT_PIT_P" : 0.15,
"Q_A_RAT_PIT_I" : 0.25,
"Q_A_RAT_PIT_D" : 0.002,
"Q_A_RAT_YAW_P" : 0.18,
"Q_A_RAT_YAW_I" : 0.018,
"Q_A_ANG_RLL_P" : 4.5,
"Q_A_ANG_PIT_P" : 4.5,
})
# this is a list of all parameters modified by QAUTOTUNE. Set
# them so that when the context is popped we get the original
# values back:
@ -1411,6 +1425,81 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
self.wait_disarmed(timeout=120)
def VTOLQuicktune_CPP(self):
'''VTOL Quicktune in C++'''
self.set_parameters({
"RC7_OPTION": 181,
"QWIK_ENABLE" : 1,
"QWIK_DOUBLE_TIME" : 5, # run faster for autotest
})
self.context_push()
self.context_collect('STATUSTEXT')
# reduce roll/pitch gains by 2
gain_mul = 0.5
soften_params = ['Q_A_RAT_RLL_P', 'Q_A_RAT_RLL_I', 'Q_A_RAT_RLL_D',
'Q_A_RAT_PIT_P', 'Q_A_RAT_PIT_I', 'Q_A_RAT_PIT_D',
'Q_A_RAT_YAW_P', 'Q_A_RAT_YAW_I']
original_values = self.get_parameters(soften_params)
softened_values = {}
for p in original_values.keys():
softened_values[p] = original_values[p] * gain_mul
self.set_parameters(softened_values)
self.wait_ready_to_arm()
self.change_mode("QLOITER")
self.set_rc(7, 1000)
self.arm_vehicle()
self.takeoff(20, 'QLOITER')
# use rc switch to start tune
self.set_rc(7, 1500)
self.wait_text("Quicktune: starting tune", check_context=True)
for axis in ['Roll', 'Pitch', 'Yaw']:
self.wait_text("Starting %s tune" % axis, check_context=True)
self.wait_text("Quicktune: %s D done" % axis, check_context=True, timeout=120)
self.wait_text("Quicktune: %s P done" % axis, check_context=True, timeout=120)
self.wait_text("Quicktune: %s done" % axis, check_context=True, timeout=120)
new_values = self.get_parameters(soften_params)
for p in original_values.keys():
threshold = 0.8 * original_values[p]
self.progress("tuned param %s %.4f need %.4f" % (p, new_values[p], threshold))
if new_values[p] < threshold:
raise NotAchievedException(
"parameter %s %.4f not increased over %.4f" %
(p, new_values[p], threshold))
self.progress("ensure we are not overtuned")
self.set_parameter('SIM_ENGINE_MUL', 0.9)
self.delay_sim_time(5)
# and restore it
self.set_parameter('SIM_ENGINE_MUL', 1)
for i in range(5):
self.wait_heartbeat()
if self.statustext_in_collections("ABORTING"):
raise NotAchievedException("tune has aborted, overtuned")
self.progress("using aux fn for save tune")
# to test aux function method, use aux fn for save
self.run_auxfunc(181, 2)
self.wait_text("Quicktune: saved", check_context=True)
self.change_mode("QLAND")
self.wait_disarmed(timeout=120)
self.set_parameter("QWIK_ENABLE", 0)
self.context_pop()
self.reboot_sitl()
def PrecisionLanding(self):
'''VTOL precision landing'''
@ -2104,6 +2193,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
self.LoiterAltQLand,
self.VTOLLandSpiral,
self.VTOLQuicktune,
self.VTOLQuicktune_CPP,
self.PrecisionLanding,
self.ShipLanding,
Test(self.MotorTest, kwargs={ # tests motors 4 and 2

70
Tools/autotest/rover.py
View File

@ -6876,6 +6876,75 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.set_parameter("BATT_MONITOR", 0)
self.wait_ready_to_arm()
# this method modified from cmd_addpoly in the MAVProxy code:
def generate_polyfence(self, centre_loc, command, radius, count, rotation=0):
'''adds a number of waypoints equally spaced around a circle
'''
if count < 3:
raise ValueError("Invalid count (%s)" % str(count))
if radius <= 0:
raise ValueError("Invalid radius (%s)" % str(radius))
latlon = (centre_loc.lat, centre_loc.lng)
items = []
for i in range(0, count):
(lat, lon) = mavextra.gps_newpos(latlon[0],
latlon[1],
360/float(count)*i + rotation,
radius)
m = mavutil.mavlink.MAVLink_mission_item_int_message(
1, # target system
1, # target component
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL, # frame
command, # command
0, # current
0, # autocontinue
count, # param1,
0.0, # param2,
0.0, # param3
0.0, # param4
int(lat*1e7), # x (latitude)
int(lon*1e7), # y (longitude)
0, # z (altitude)
mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
)
items.append(m)
return items
def SDPolyFence(self):
'''test storage of fence on SD card'''
self.set_parameters({
'BRD_SD_FENCE': 32767,
})
self.reboot_sitl()
home = self.home_position_as_mav_location()
fence = self.generate_polyfence(
home,
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
radius=100,
count=100,
)
for bearing in range(0, 359, 60):
x = self.offset_location_heading_distance(home, bearing, 100)
fence.extend(self.generate_polyfence(
x,
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
radius=100,
count=100,
))
self.correct_wp_seq_numbers(fence)
self.check_fence_upload_download(fence)
self.delay_sim_time(1000)
def tests(self):
'''return list of all tests'''
ret = super(AutoTestRover, self).tests()
@ -6927,6 +6996,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.DataFlash,
self.SkidSteer,
self.PolyFence,
self.SDPolyFence,
self.PolyFenceAvoidance,
self.PolyFenceObjectAvoidanceAuto,
self.PolyFenceObjectAvoidanceGuided,

18
Tools/autotest/vehicle_test_suite.py
View File

@ -2711,8 +2711,6 @@ class TestSuite(ABC):
"SIM_MAG1_OFS_Z",
"SIM_PARA_ENABLE",
"SIM_PARA_PIN",
"SIM_PLD_ALT_LMT",
"SIM_PLD_DIST_LMT",
"SIM_RICH_CTRL",
"SIM_RICH_ENABLE",
"SIM_SHIP_DSIZE",
@ -3320,7 +3318,7 @@ class TestSuite(ABC):
os.link(self.logfile, self.buildlog)
except OSError as error:
self.progress("OSError [%d]: %s" % (error.errno, error.strerror))
self.progress("WARN: Failed to create link: %s => %s, "
self.progress("Problem: Failed to create link: %s => %s, "
"will copy tlog manually to target location" %
(self.logfile, self.buildlog))
self.copy_tlog = True
@ -9430,6 +9428,20 @@ Also, ignores heartbeats not from our target system'''
location.alt,
location.heading)
def offset_location_heading_distance(self, location, bearing, distance):
(target_lat, target_lng) = mavextra.gps_newpos(
location.lat,
location.lng,
bearing,
distance
)
return mavutil.location(
target_lat,
target_lng,
location.alt,
location.heading
)
def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
tstart = self.get_sim_time()
while True:

BIN
Tools/bootloaders/F4BY_F427_bl.bin generated Executable file
View File

Binary file not shown.

2
Tools/environment_install/install-prereqs-ubuntu.sh
View File

@ -164,7 +164,7 @@ fi
# Lists of packages to install
BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect astyle"
PYTHON_PKGS="future lxml pymavlink pyserial MAVProxy geocoder empy==3.3.4 ptyprocess dronecan"
PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser wsproto"
PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser wsproto tabulate"
# add some Python packages required for commonly-used MAVProxy modules and hex file generation:
if [[ $SKIP_AP_EXT_ENV -ne 1 ]]; then

180
Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py Executable file
View File

@ -0,0 +1,180 @@
#!/usr/bin/env python3
# Copyright 2023 ArduPilot.org.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
"""
Run takeoff test on Copter.
Warning - This is NOT production code; it's a simple demo of capability.
"""
import math
import rclpy
import time
import errno
from rclpy.node import Node
from builtin_interfaces.msg import Time
from ardupilot_msgs.msg import GlobalPosition
from geographic_msgs.msg import GeoPoseStamped
from geopy import distance
from geopy import point
from ardupilot_msgs.srv import ArmMotors
from ardupilot_msgs.srv import ModeSwitch
from ardupilot_msgs.srv import Takeoff
COPTER_MODE_GUIDED = 4
TAKEOFF_ALT = 10.5
class CopterTakeoff(Node):
"""Copter takeoff using guided control."""
def __init__(self):
"""Initialise the node."""
super().__init__("copter_takeoff")
self.declare_parameter("arm_topic", "/ap/arm_motors")
self._arm_topic = self.get_parameter("arm_topic").get_parameter_value().string_value
self._client_arm = self.create_client(ArmMotors, self._arm_topic)
while not self._client_arm.wait_for_service(timeout_sec=1.0):
self.get_logger().info('arm service not available, waiting again...')
self.declare_parameter("mode_topic", "/ap/mode_switch")
self._mode_topic = self.get_parameter("mode_topic").get_parameter_value().string_value
self._client_mode_switch = self.create_client(ModeSwitch, self._mode_topic)
while not self._client_mode_switch.wait_for_service(timeout_sec=1.0):
self.get_logger().info('mode switch service not available, waiting again...')
self.declare_parameter("takeoff_service", "/ap/experimental/takeoff")
self._takeoff_topic = self.get_parameter("takeoff_service").get_parameter_value().string_value
self._client_takeoff = self.create_client(Takeoff, self._takeoff_topic)
while not self._client_takeoff.wait_for_service(timeout_sec=1.0):
self.get_logger().info('takeoff service not available, waiting again...')
self.declare_parameter("geopose_topic", "/ap/geopose/filtered")
self._geopose_topic = self.get_parameter("geopose_topic").get_parameter_value().string_value
qos = rclpy.qos.QoSProfile(
reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1
)
self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
self._cur_geopose = GeoPoseStamped()
def geopose_cb(self, msg: GeoPoseStamped):
"""Process a GeoPose message."""
stamp = msg.header.stamp
if stamp.sec:
self.get_logger().info("From AP : Geopose [sec:{}, nsec: {}]".format(stamp.sec, stamp.nanosec))
# Store current state
self._cur_geopose = msg
def arm(self):
req = ArmMotors.Request()
req.arm = True
future = self._client_arm.call_async(req)
rclpy.spin_until_future_complete(self, future)
return future.result()
def arm_with_timeout(self, timeout: rclpy.duration.Duration):
"""Try to arm. Returns true on success, or false if arming fails or times out."""
armed = False
start = self.get_clock().now()
while not armed and self.get_clock().now() - start < timeout:
armed = self.arm().result
time.sleep(1)
return armed
def switch_mode(self, mode):
req = ModeSwitch.Request()
assert mode in [COPTER_MODE_GUIDED]
req.mode = mode
future = self._client_mode_switch.call_async(req)
rclpy.spin_until_future_complete(self, future)
return future.result()
def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration):
"""Try to switch mode. Returns true on success, or false if mode switch fails or times out."""
is_in_desired_mode = False
start = self.get_clock().now()
while not is_in_desired_mode:
result = self.switch_mode(desired_mode)
# Handle successful switch or the case that the vehicle is already in expected mode
is_in_desired_mode = result.status or result.curr_mode == desired_mode
time.sleep(1)
return is_in_desired_mode
def takeoff(self, alt):
req = Takeoff.Request()
req.alt = alt
future = self._client_takeoff.call_async(req)
rclpy.spin_until_future_complete(self, future)
return future.result()
def takeoff_with_timeout(self, alt: int, timeout: rclpy.duration.Duration):
"""Try to takeoff. Returns true on success, or false if takeoff fails or times out."""
takeoff_success = False
start = self.get_clock().now()
while not takeoff_success:
result = self.takeoff(alt)
takeoff_success = result.status
time.sleep(1)
return takeoff_success
def get_cur_geopose(self):
"""Return latest geopose."""
return self._cur_geopose
def main(args=None):
"""Node entry point."""
rclpy.init(args=args)
node = CopterTakeoff()
try:
if not node.switch_mode_with_timeout(COPTER_MODE_GUIDED, rclpy.duration.Duration(seconds=20)):
raise RuntimeError("Unable to switch to guided mode")
# Block till armed, which will wait for EKF3 to initialize
if not node.arm_with_timeout(rclpy.duration.Duration(seconds=30)):
raise RuntimeError("Unable to arm")
# Block till in takeoff
if not node.takeoff_with_timeout(TAKEOFF_ALT, rclpy.duration.Duration(seconds=20)):
raise RuntimeError("Unable to takeoff")
is_ascending_to_takeoff_alt = True
while is_ascending_to_takeoff_alt:
rclpy.spin_once(node)
time.sleep(1.0)
is_ascending_to_takeoff_alt = node.get_cur_geopose().pose.position.altitude < TAKEOFF_ALT
if is_ascending_to_takeoff_alt:
raise RuntimeError("Failed to reach takeoff altitude")
except KeyboardInterrupt:
pass
# Destroy the node explicitly.
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

1
Tools/ros2/ardupilot_dds_tests/setup.py
View File

@ -27,6 +27,7 @@ setup(
"time_listener = ardupilot_dds_tests.time_listener:main",
"plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main",
"pre_arm_check = ardupilot_dds_tests.pre_arm_check_service:main",
"copter_takeoff = ardupilot_dds_tests.copter_takeoff:main",
],
},
)

1
Tools/ros2/ardupilot_msgs/CMakeLists.txt
View File

@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Status.msg"
"srv/ArmMotors.srv"
"srv/ModeSwitch.srv"
"srv/Takeoff.srv"
DEPENDENCIES geometry_msgs std_msgs
ADD_LINTER_TESTS
)

10
Tools/ros2/ardupilot_msgs/srv/Takeoff.srv Normal file
View File

@ -0,0 +1,10 @@
# This service requests the vehicle to takeoff
# alt : Set the takeoff altitude above home or above terrain(in case of rangefinder)
float32 alt
---
# status : True if the request for mode switch was successful, False otherwise
bool status

16
Tools/scripts/build_ci.sh
View File

@ -5,7 +5,13 @@
XOLDPWD=$PWD # profile changes directory :-(
. ~/.profile
if [ -z "$GITHUB_ACTIONS" ] || [ "$GITHUB_ACTIONS" != "true" ]; then
. ~/.profile
fi
if [ "$CI" = "true" ]; then
export PIP_ROOT_USER_ACTION=ignore
fi
cd $XOLDPWD
@ -40,7 +46,7 @@ function install_pymavlink() {
if [ $pymavlink_installed -eq 0 ]; then
echo "Installing pymavlink"
git submodule update --init --recursive --depth 1
(cd modules/mavlink/pymavlink && python3 -m pip install --user .)
(cd modules/mavlink/pymavlink && python3 -m pip install --progress-bar off --cache-dir /tmp/pip-cache --user .)
pymavlink_installed=1
fi
}
@ -51,12 +57,12 @@ function install_mavproxy() {
pushd /tmp
git clone https://github.com/ardupilot/MAVProxy --depth 1
pushd MAVProxy
python3 -m pip install --user --force .
python3 -m pip install --progress-bar off --cache-dir /tmp/pip-cache --user --force .
popd
popd
mavproxy_installed=1
# now uninstall the version of pymavlink pulled in by MAVProxy deps:
python3 -m pip uninstall -y pymavlink
python3 -m pip uninstall -y pymavlink --cache-dir /tmp/pip-cache
fi
}
@ -458,7 +464,7 @@ for t in $CI_BUILD_TARGET; do
echo "Building signed firmwares"
sudo apt-get update
sudo apt-get install -y python3-dev
python3 -m pip install pymonocypher==3.1.3.2
python3 -m pip install pymonocypher==3.1.3.2 --progress-bar off --cache-dir /tmp/pip-cache
./Tools/scripts/signing/generate_keys.py testkey
$waf configure --board CubeOrange-ODID --signed-fw --private-key testkey_private_key.dat
$waf copter

1
Tools/scripts/build_options.py
View File

@ -224,6 +224,7 @@ BUILD_OPTIONS = [
Feature('Plane', 'AP_TX_TUNING', 'AP_TUNING_ENABLED', 'Enable TX-based tuning parameter adjustments', 0, None),
Feature('Plane', 'PLANE_GUIDED_SLEW', 'AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', 'Enable Offboard-guided slew commands', 0, None), # noqa:401
Feature('Plane', 'PLANE_GLIDER_PULLUP', 'AP_PLANE_GLIDER_PULLUP_ENABLED', 'Enable Glider pullup support', 0, None),
Feature('Plane', 'QUICKTUNE', 'AP_QUICKTUNE_ENABLED', 'Enable VTOL quicktune', 0, None),
Feature('RC', 'RC_Protocol', 'AP_RCPROTOCOL_ENABLED', "Enable Serial RC Protocols", 0, None), # NOQA: E501
Feature('RC', 'RC_CRSF', 'AP_RCPROTOCOL_CRSF_ENABLED', "Enable CRSF", 0, "RC_Protocol"), # NOQA: E501

15
Tools/scripts/pretty_diff_size.py → Tools/scripts/build_tests/pretty_diff_size.py
View File

@ -90,6 +90,21 @@ def print_table(summary_data_list_second, summary_data_list_master):
print_data.append(col_data)
print(tabulate(print_data, headers=["Binary Name", "Text [B]", "Data [B]", "BSS (B)",
"Total Flash Change [B] (%)", "Flash Free After PR (B)"]))
# Get the GitHub Actions summary file path
summary_file = os.getenv('GITHUB_STEP_SUMMARY')
if summary_file:
# Append the output to the summary file
with open(summary_file, 'a') as f:
f.write("### Diff summary\n")
f.write(tabulate(print_data, headers=[
"Binary Name",
"Text [B]",
"Data [B]",
"BSS (B)",
"Total Flash Change [B] (%)",
"Flash Free After PR (B)"
], tablefmt="github"))
f.write("\n")
def extract_binaries_size(path):

13
Tools/scripts/build_tests/test_ccache.py
View File

@ -65,11 +65,22 @@ subprocess.run(["ccache", "-C", "-z"])
build_board(boards[0])
subprocess.run(["ccache", "-z"])
build_board(boards[1])
subprocess.run(["ccache", "-s"])
result = subprocess.run(["ccache", "-s"], capture_output=True, text=True)
print(result.stdout)
# Get the GitHub Actions summary file path
summary_file = os.getenv('GITHUB_STEP_SUMMARY')
post = ccache_stats()
hit_pct = 100 * post[0] / float(post[0]+post[1])
print("ccache hit percentage: %.1f%% %s" % (hit_pct, post))
if summary_file:
# Append the output to the summary file
with open(summary_file, 'a') as f:
f.write(f"### ccache -s Output with {boards}\n")
f.write(f"```\n{result.stdout}\n```\n")
f.write(f"### ccache hit percentage (min {args.min_cache_pct})\n")
f.write("ccache hit percentage: %.1f%% %s\n" % (hit_pct, post))
if hit_pct < args.min_cache_pct:
print("ccache hits too low, need %d%%" % args.min_cache_pct)
sys.exit(1)

1
Tools/scripts/extract_features.py
View File

@ -281,6 +281,7 @@ class ExtractFeatures(object):
('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'),
('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'),
('AP_QUICKTUNE_ENABLED', r'AP_Quicktune::update'),
]
def progress(self, msg):

12
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -306,7 +306,7 @@ bool AC_PolyFence_loader::formatted() const
uint16_t AC_PolyFence_loader::max_items() const
{
// this is 84 items on PixHawk
return MIN(255U, fence_storage.size() / sizeof(Vector2l));
return fence_storage.size() / sizeof(Vector2l);
}
bool AC_PolyFence_loader::format()
@ -650,7 +650,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
// FIXME: find some way of factoring out all of these allocation routines.
{ // allocate storage for inclusion polyfences:
const uint8_t count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION);
const auto count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION);
Debug("Fence: Allocating %u bytes for inc. fences",
(unsigned)(count * sizeof(InclusionBoundary)));
_loaded_inclusion_boundary = NEW_NOTHROW InclusionBoundary[count];
@ -662,7 +662,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
{ // allocate storage for exclusion polyfences:
const uint8_t count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION);
const auto count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION);
Debug("Fence: Allocating %u bytes for exc. fences",
(unsigned)(count * sizeof(ExclusionBoundary)));
_loaded_exclusion_boundary = NEW_NOTHROW ExclusionBoundary[count];
@ -674,7 +674,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
{ // allocate storage for circular inclusion fences:
uint8_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION);
uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION);
count += index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION_INT)
Debug("Fence: Allocating %u bytes for circ. inc. fences",
(unsigned)(count * sizeof(InclusionCircle)));
@ -687,7 +687,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
{ // allocate storage for circular exclusion fences:
uint8_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION);
uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION);
count += index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION_INT)
Debug("Fence: Allocating %u bytes for circ. exc. fences",
(unsigned)(count * sizeof(ExclusionCircle)));
@ -1060,7 +1060,7 @@ bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_
return false;
}
uint8_t total_vertex_count = 0;
uint16_t total_vertex_count = 0;
uint16_t offset = 4; // skipping magic
uint8_t vertex_count = 0;
for (uint16_t i=0; i<count; i++) {

2
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -9,7 +9,7 @@
// radius looks like an integer as a backwards-compatibility measure.
// For 4.2 we might consider only loading _INT and always saving as
// float, and in 4.3 considering _INT invalid
enum class AC_PolyFenceType {
enum class AC_PolyFenceType : uint8_t {
END_OF_STORAGE = 99,
POLYGON_INCLUSION = 98,
POLYGON_EXCLUSION = 97,

34
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -29,6 +29,9 @@
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
#include "std_srvs/srv/Trigger.h"
#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#include "ardupilot_msgs/srv/Takeoff.h"
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h"
@ -915,6 +918,35 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
break;
}
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: {
ardupilot_msgs_srv_Takeoff_Request takeoff_request;
ardupilot_msgs_srv_Takeoff_Response takeoff_response;
const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request);
if (deserialize_success == false) {
break;
}
takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt);
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id,
.type = UXR_REPLIER_ID
};
uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL");
break;
}
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: {
std_srvs_srv_Trigger_Request prearm_check_request;
@ -943,7 +975,7 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
break;
}
#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED
#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);

15
libraries/AP_DDS/AP_DDS_Service_Table.h
View File

@ -11,6 +11,9 @@ enum class ServiceIndex: uint8_t {
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
PREARM_CHECK,
#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
TAKEOFF,
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
SET_PARAMETERS,
GET_PARAMETERS
@ -66,6 +69,18 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
.reply_topic_name = "rr/ap/prearm_checkReply",
},
#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
{
.req_id = to_underlying(ServiceIndex::TAKEOFF),
.rep_id = to_underlying(ServiceIndex::TAKEOFF),
.service_rr = Service_rr::Replier,
.service_name = "rs/ap/experimental/takeoffService",
.request_type = "ardupilot_msgs::srv::dds_::Takeoff_Request_",
.reply_type = "ardupilot_msgs::srv::dds_::Takeoff_Response_",
.request_topic_name = "rq/ap/experimental/takeoffRequest",
.reply_topic_name = "rr/ap/experimental/takeoffReply",
},
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
{
.req_id = to_underlying(ServiceIndex::SET_PARAMETERS),

4
libraries/AP_DDS/AP_DDS_config.h
View File

@ -141,6 +141,10 @@
#define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1
#endif
#ifndef AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
#define AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED 1
#endif
#ifndef AP_DDS_PARAMETER_SERVER_ENABLED
#define AP_DDS_PARAMETER_SERVER_ENABLED 1
#endif

20
libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl Normal file
View File

@ -0,0 +1,20 @@
// generated from rosidl_adapter/resource/srv.idl.em
// with input from ardupilot_msgs/srv/Takeoff.srv
// generated code does not contain a copyright notice
module ardupilot_msgs {
module srv {
struct Takeoff_Request {
@verbatim (language="comment", text=
"This service requests the vehicle to takeoff" "\n"
"float : Set the takeoff altitude [m] above home, or above terrain if rangefinder is healthy")
float alt;
};
@verbatim (language="comment", text=
"status : True if the request for takeoff was successful, False otherwise")
struct Takeoff_Response {
boolean status;
};
};
};

14
libraries/AP_DDS/README.md
View File

@ -210,6 +210,7 @@ $ ros2 service list
/ap/arm_motors
/ap/mode_switch
/ap/prearm_check
/ap/experimental/takeoff
---
```
@ -236,6 +237,7 @@ $ ros2 service list -t
/ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
/ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
/ap/prearm_check [std_srvs/srv/Trigger]
/ap/experimental/takeoff [ardupilot_msgs/srv/Takeoff]
```
Call the arm motors service:
@ -272,6 +274,16 @@ or
std_srvs.srv.Trigger_Response(success=True, message='Vehicle is Armable')
```
Call the takeoff service:
```bash
$ ros2 service call /ap/experimental/takeoff ardupilot_msgs/srv/Takeoff "{alt: 10.5}"
requester: making request: ardupilot_msgs.srv.Takeoff_Request(alt=10.5)
response:
ardupilot_msgs.srv.Takeoff_Response(status=True)
```
## Commanding using ROS 2 Topics
The following topic can be used to control the vehicle.
@ -296,7 +308,7 @@ ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34
publisher: beginning loop
publishing #1: ardupilot_msgs.msg.GlobalPosition(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), coordinate_frame=0, type_mask=0, latitude=34.0, longitude=118.0, altitude=1000.0, velocity=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), acceleration_or_force=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), yaw=0.0)
```
## Contributing to `AP_DDS` library
### Adding DDS messages to Ardupilot

4
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -77,7 +77,7 @@ for FrSky SPort Passthrough
#define WIND_ANGLE_LIMIT 0x7F
#define WIND_SPEED_OFFSET 7
#define WIND_APPARENT_ANGLE_OFFSET 15
#define WIND_APPARENT_SPEED_OFFSET 23
#define WIND_APPARENT_SPEED_OFFSET 22
// for waypoint data
#define WP_NUMBER_LIMIT 2047
#define WP_DISTANCE_LIMIT 1023000
@ -781,7 +781,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_wind(void)
// true wind speed in dm/s
value |= prep_number(roundf(windvane->get_true_wind_speed() * 10), 2, 1) << WIND_SPEED_OFFSET;
// apparent wind angle in 3 degree increments -180,180 (signed)
value |= prep_number(roundf(degrees(windvane->get_apparent_wind_direction_rad()) * (1.0f/3.0f)), 2, 0);
value |= prep_number(roundf(degrees(windvane->get_apparent_wind_direction_rad()) * (1.0f/3.0f)), 2, 0) << WIND_APPARENT_ANGLE_OFFSET;
// apparent wind speed in dm/s
value |= prep_number(roundf(windvane->get_apparent_wind_speed() * 10), 2, 1) << WIND_APPARENT_SPEED_OFFSET;
}

4
libraries/AP_HAL/board/esp32.h
View File

@ -52,6 +52,10 @@
#define __LITTLE_ENDIAN 1234
#define __BYTE_ORDER __LITTLE_ENDIAN
// ArduPilot uses __RAMFUNC__ to place functions in fast instruction RAM
#define __RAMFUNC__ IRAM_ATTR
// whenver u get ... error: "xxxxxxx" is not defined, evaluates to 0 [-Werror=undef] just define it below as 0
#define CONFIG_SPIRAM_ALLOW_BSS_SEG_EXTERNAL_MEMORY 0
#define XCHAL_ERRATUM_453 0

BIN
libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743Pinout.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.4 MiB

BIN
libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743StampFront&Back.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.6 MiB

BIN
libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743_SD.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.5 MiB

24
libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/README.md
View File

@ -2,19 +2,19 @@
# CBUnmanned H743 Stamp
The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller loosely based on the FMUv6 standards & is designed for low volume OEMs as a drop in way to add ArduPilot to their custom hardware builds. It is a part of CBUnmanned's wider ["Stamp" Eco-System](https://wiki.cbunmanned.com/wiki/cbunmanned-stamp-eco-system), which brings together all the typical avionics hardware into a neat custom carrier PCB. Mounting footprints and symbols are available along with examples of basic usage on the [Wiki](https://wiki.cbunmanned.com/wiki/cbunmanned-stamp-eco-system/h743-flight-controller).
The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller loosely based on the FMUv6 standards & is designed for low volume OEMs as a drop in way to add ArduPilot to their custom hardware builds. It is a part of CBUnmanned's wider ["Stamp" Eco-System](https://cbunmanned.com), which brings together all the typical avionics hardware into a neat custom carrier PCB. Mounting footprints and symbols are available along with examples of basic usage on the [Wiki](https://wiki.cbunmanned.com/).
![H743StampFront&Back](H743StampFront&Back.png "H743FB")
![H743StampFront&Back](H743_SD.png "H743FB")
## Features
- Class leading H7 SOC.
- Triple IMU sensors for extra redundancy.
- Based on the FMU-V6 standards.
- Micro SD Card for Logging/LUA Scripting.
- Direct solder mounting or optional 1.27mm header.
- 1.27mm header
- x1 Ethernet and x2 CAN for easy integration with the next generation of UAV accessories.
- All complicated/supporting circuitry is on-board, just power with 5v.
- Just 22mm x 24.25mm & 1.9g.
- Just 22mm x 24.25mm & 3g.
## Specifications
- Processor
@ -26,7 +26,7 @@ The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller
- Sensors
- x2 Ivensense ICM-42688 IMU
- x1 Ivensense ICM-42670 IMU
- x1 Infineon DPS310 Barometer
- x1 BMP280 Barometer
- x1 Bosch BMM150 Magnetometer
- Power
@ -50,7 +50,7 @@ The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller
![H743 Stamp Pinout](H743Pinout.png "H743")
### UART Mapping (Yellow Fade)
### UART Mapping
Ardupilot -> STM32
- SERIAL0 -> USB
@ -75,10 +75,10 @@ USART 6 Tx is available for use with bi directional protocols.
An optional IOMCU can be connected to this serial port, a compatible custom build of the firmware required.
### CAN Ports (Light Green Fade)
### CAN Ports
2 CAN buses are available, each with a built in 120 ohm termination resistor.
### I2C (Maroon Fade)
### I2C
I2C 1 - Internal for BMM150 Compass.
I2C 2 - Internal for DPS310 Barometer.
@ -87,10 +87,10 @@ I2C 3 - External With internal 2.2k Pull Up.
I2C 4 - External With internal 2.2k Pull Up.
### SPI (Cyan Fade)
### SPI
SPI 4 is available for use with external sensors alongside a Chip Select and Data Ready pin, compatible custom build of the firmware required.
### PWM Output (Blue Fade)
### PWM Output
The Stamp supports up to 10 PWM outputs with D-Shot.
The PWM outputs are in 3 groups:
@ -105,11 +105,11 @@ BiDirectional DShot available on the first 8 outputs.
A buzzer alarm signal is available on Timer 14.
### Analog Inputs (Purple Fade)
### Analog Inputs
The board has two ADC input channels for Voltage (0-3.3v) and Current (0-3.3v) measurement. Settings are dependent on the external hardware used.
### Ethernet (Green Fade)
### Ethernet
Ethernet is available on 4 output pads and has internal magnetics supporting direct connection to external equipment, no need for a large RJ45 connector.
### Compass

4
libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat
View File

@ -149,7 +149,7 @@ PB13 CAN2_TX CAN2
PB9 I2C1_SDA I2C1
PB8 I2C1_SCL I2C1
# I2C2 - DPS310
# I2C2 - BMP280
PF1 I2C2_SCL I2C2
PF0 I2C2_SDA I2C2
@ -193,7 +193,7 @@ define AP_NOTIFY_GPIO_LED_1_PIN 90
define HAL_GPIO_LED_ON 1
# barometers
BARO DPS310 I2C:1:0x77
BARO BMP280 I2C:1:0x76
# compass
COMPASS BMM150 I2C:0:0x10 false ROTATION_YAW_180

11
libraries/AP_HAL_ChibiOS/hwdef/CubeNode-ETH/hwdef.dat
View File

@ -3,7 +3,7 @@ include ../CubeNode/hwdef.dat
# we need RTS/CTS for the PPP link
undef PE0
undef PE1
undef PC11
undef PC12
undef HAL_PERIPH_ENABLE_IMU
undef HAL_GCS_ENABLED
@ -11,12 +11,17 @@ undef HAL_GCS_ENABLED
PE1 UART8_TX UART8
PE0 UART8_RX UART8
PA10 UART8_RTS UART8
PC11 UART8_CTS_GPIO UART8
PC12 UART8_CTS_GPIO UART8
SERIAL_ORDER OTG1 UART8
PA4 VDD_5V_SENS ADC1 SCALE(2)
# LEDs
undef PC11
PC11 LED0 OUTPUT LOW GPIO(0)
PB14 LED1 OUTPUT LOW GPIO(1)
undef HAL_USE_ADC
define HAL_USE_ADC TRUE
define HAL_WITH_MCU_MONITORING 1
@ -29,3 +34,5 @@ include ../include/network_PPPGW.inc
define HAL_MONITOR_THREAD_ENABLED 1
define AP_NETWORKING_TESTS_ENABLED 1
define HAL_PERIPH_SHOW_SERIAL_MANAGER_PARAMS 1

22
libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/README.md Normal file
View File

@ -0,0 +1,22 @@
# F4BY Flight Controller MCU upgrade
The F4BY flight controller shop: https://f4by.com/en/?order/our_product
The instructions are available here: https://f4by.com/en/?doc/fc_f4by_v2.1.5
## Howto
for self upgrage old fc:
replace old MCU STM32F407VGT (1MB Flash) with STM32F427VET rev3 or above (2MB Flash)
## Features
- Full Ardupilot features support (exclude LUA Script)
## Loading Firmware
Initial firmware load can be done with DFU by plugging in USB with the
boot solder pads connected. Then you should load the "with_bl.hex"
firmware, using your favourite DFU loading tool.

42
libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef-bl.dat Normal file
View File

@ -0,0 +1,42 @@
# hw definition file for processing by chibios_hwdef.py
# for f4by bootloader
MCU STM32F4xx STM32F427xx
APJ_BOARD_ID AP_HW_F4BY_F427
OSCILLATOR_HZ 8000000
STM32_ST_USE_TIMER 5
# flash size
FLASH_SIZE_KB 2048
# order of UARTs (and USB)
SERIAL_ORDER OTG1 USART2
PE3 LED_BOOTLOADER OUTPUT
PE2 LED_ACTIVITY OUTPUT
define HAL_LED_ON 1
PA9 VBUS INPUT
PA11 OTG_FS_DM OTG1
PA12 OTG_FS_DP OTG1
PA13 JTMS-SWDIO SWD
PA14 JTCK-SWCLK SWD
PD5 USART2_TX USART2
PD6 USART2_RX USART2
FLASH_USE_MAX_KB 16
FLASH_RESERVE_START_KB 0
# location of application code
FLASH_BOOTLOADER_LOAD_KB 16
# Add CS pins to ensure they are high in bootloader
PA4 MPU_CS CS
PB12 FRAM_CS CS SPEED_VERYLOW
PE15 FLASH_CS CS

177
libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef.dat Normal file
View File

@ -0,0 +1,177 @@
# hw definition file for processing by chibios_hwdef.py
# for F4BY v2.1.5 board description http://swift-flyer.com/?page_id=83
# MCU class and specific type
MCU STM32F4xx STM32F427xx
# board ID for firmware load
APJ_BOARD_ID AP_HW_F4BY_F427
# USB setup
USB_VENDOR 0x27AC # Swift-Flyer
USB_PRODUCT 0x0201 # fmu usb driver
USB_STRING_MANUFACTURER "Swift-Flyer"
PA11 OTG_FS_DM OTG1
PA12 OTG_FS_DP OTG1
# this is the pin that senses USB being connected. It is an input pin
# setup as OPENDRAIN
PA9 VBUS INPUT OPENDRAIN
FLASH_SIZE_KB 2048
FLASH_RESERVE_START_KB 16
define HAL_STORAGE_SIZE 16384
env OPTIMIZE -O2
# crystal frequency
OSCILLATOR_HZ 8000000
# ChibiOS system timer
STM32_ST_USE_TIMER 5
PA13 JTMS-SWDIO SWD
PA14 JTCK-SWCLK SWD
# the normal usage of this ordering is:
# 1) SERIAL0: console (primary mavlink, usually USB)
# 2) SERIAL3: primary GPS
# 3) SERIAL1: telem1
# 4) SERIAL2: telem2
# 5) SERIAL4: GPS2
# 6) SERIAL5: extra UART (usually RTOS debug console)
# use UART for stdout, so no STDOUT_SERIAL
#STDOUT_SERIAL SD5
#STDOUT_BAUDRATE 57600
SERIAL_ORDER OTG1 USART2 USART1 USART3 UART4 UART5
# UART1 as board 2.1.5 for serial 3 gps
PB6 USART1_TX USART1
PB7 USART1_RX USART1
# USART2 serial2 telem2
PD5 USART2_TX USART2 NODMA
PD6 USART2_RX USART2
# USART3 serial3 telem1
PD8 USART3_TX USART3 NODMA
PD9 USART3_RX USART3
PC10 UART4_TX UART4 NODMA
PC11 UART4_RX UART4
# SHARE dma with I2C2_TX
PC12 UART5_TX UART5 NODMA
PD2 UART5_RX UART5 NODMA
#SPI1 for MPU
PA5 SPI1_SCK SPI1
PA6 SPI1_MISO SPI1
PA7 SPI1_MOSI SPI1
PA4 MPU_CS CS
# spi bus for dataflash AND SD
PB13 SPI2_SCK SPI2
PB14 SPI2_MISO SPI2
PB15 SPI2_MOSI SPI2
PB12 FRAM_CS CS SPEED_VERYLOW
PE15 FLASH_CS CS
SPIDEV mpu6000 SPI1 DEVID1 MPU_CS MODE3 1*MHZ 8*MHZ
SPIDEV ramtron SPI2 DEVID2 FRAM_CS MODE3 8*MHZ 8*MHZ
SPIDEV sdcard SPI2 DEVID3 FLASH_CS MODE0 400*KHZ 25*MHZ
# one IMU
IMU Invensense SPI:mpu6000 ROTATION_NONE
# one baro, check both addresses
BARO MS56XX I2C:0:0x76
BARO MS56XX I2C:0:0x77
# enable RAMTROM parameter storage
define HAL_WITH_RAMTRON 1
# enable FAT filesystem support
define HAL_OS_FATFS_IO 1
# this defines the default maximum clock on I2C devices.
define HAL_I2C_MAX_CLOCK 100000
I2C_ORDER I2C2 I2C1
PB8 I2C1_SCL I2C1
PB9 I2C1_SDA I2C1
PB10 I2C2_SCL I2C2
PB11 I2C2_SDA I2C2
# look for I2C compass
COMPASS HMC5843 I2C:0:0x1E false ROTATION_YAW_270
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
# PWM out pins
PA0 TIM2_CH1 TIM2 PWM(1) GPIO(50)
PA1 TIM2_CH2 TIM2 PWM(2) GPIO(51)
PA2 TIM2_CH3 TIM2 PWM(3) GPIO(52)
PA3 TIM2_CH4 TIM2 PWM(4) GPIO(53)
PE9 TIM1_CH1 TIM1 PWM(5) GPIO(54)
PE11 TIM1_CH2 TIM1 PWM(6) GPIO(55)
PE13 TIM1_CH3 TIM1 PWM(7) GPIO(56)
PE14 TIM1_CH4 TIM1 PWM(8) GPIO(57)
PD13 TIM4_CH2 TIM4 PWM(9) GPIO(58)
PD12 TIM4_CH1 TIM4 PWM(10) GPIO(59)
PD15 TIM4_CH4 TIM4 PWM(11) GPIO(60)
PD14 TIM4_CH3 TIM4 PWM(12) GPIO(61)
# also USART6_RX for unidirectional RC,including PPM
PC7 TIM8_CH2 TIM8 RCININT PULLDOWN LOW DMA_CH0
# PC6 TIM8_CH1 TIM8 RCININT PULLDOWN LOW DMA_CH0
# New style Pixracer LED configuration for master repo
define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1
PE3 LED_RED OUTPUT GPIO(10)
PE2 LED_GREEN OUTPUT GPIO(11)
PE1 LED_BLUE OUTPUT GPIO(12)
PE0 LED_YELOW OUTPUT GPIO(13)
define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 10
define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 11
define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 12
PC0 PRESSURE_SENS ADC1 SCALE(1)
PC1 RSSI_IN ADC1
PC2 BATT_CURRENT_SENS ADC1 SCALE(2)
PC3 BATT_VOLTAGE_SENS ADC1 SCALE(2)
PC4 VDD_5V_SENS ADC1 SCALE(2)
PC5 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(2)
PE5 TIM9_CH1 TIM9 ALARM
PC14 EXTERN_GPIO1 OUTPUT GPIO(1)
PC13 EXTERN_GPIO2 OUTPUT GPIO(2)
PE4 EXTERN_GPIO3 OUTPUT GPIO(3)
PE6 EXTERN_GPIO4 OUTPUT GPIO(4)
PC9 EXTERN_GPIO5 OUTPUT GPIO(5)
# IRQ for MPU6000
PB0 EXTI_MPU6000 INPUT PULLUP
PB1 DRDY_HMC5883 INPUT PULLUP
# this constants with HAL_
define HAL_BATT_MONITOR_DEFAULT 4
define HAL_BATT_VOLT_PIN 13
define HAL_BATT_CURR_PIN 12
define HAL_BATT_VOLT_SCALE 16.04981
define HAL_BATT_CURR_SCALE 100
# this constant with AP_
define AP_BATT_CURR_AMP_OFFSET_DEFAULT 2.316

2
libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat
View File

@ -139,8 +139,10 @@ IMU Invensense SPI:mpu6000 ROTATION_YAW_180
# one baro
BARO BMP280 I2C:0:0x76
BARO SPL06 I2C:0:0x76
define AP_BARO_BACKEND_DEFAULT_ENABLED 0
define AP_BARO_BMP280_ENABLED 1
define AP_BARO_SPL06_ENABLED 1
# enable logging to dataflash
define HAL_LOGGING_DATAFLASH_ENABLED 1

1
libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-Wing/hwdef.dat
View File

@ -204,6 +204,7 @@ IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_270
# BMP280 integrated on I2C4 bus
BARO BMP280 I2C:0:0x76
BARO SPL06 I2C:0:0x76
define HAL_OS_FATFS_IO 1

1
libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat
View File

@ -150,6 +150,7 @@ IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180_YAW_270
# one BARO
BARO BMP280 I2C:0:0x76
BARO SPL06 I2C:0:0x76
define HAL_OS_FATFS_IO 1

1
libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat
View File

@ -153,6 +153,7 @@ IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90
# one BARO
BARO BMP280 I2C:0:0x76
BARO SPL06 I2C:0:0x76
# setup for OSD
define OSD_ENABLED 1

2
libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat
View File

@ -182,13 +182,11 @@ include ../include/minimize_fpv_osd.inc
#undef AP_LANDINGGEAR_ENABLED
#undef HAL_MOUNT_ENABLED
#undef HAL_MOUNT_SERVO_ENABLED
#undef QAUTOTUNE_ENABLED
#define AP_CAMERA_MOUNT_ENABLED 1
#define AP_LANDINGGEAR_ENABLED 1
#define HAL_MOUNT_ENABLED 1
#define HAL_MOUNT_SERVO_ENABLED 1
#define QAUTOTUNE_ENABLED 1
#define DEFAULT_NTF_LED_TYPES 257

2
libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
View File

@ -174,12 +174,10 @@ include ../include/minimize_fpv_osd.inc
undef AP_CAMERA_MOUNT_ENABLED
undef HAL_MOUNT_ENABLED
undef HAL_MOUNT_SERVO_ENABLED
undef QAUTOTUNE_ENABLED
define AP_CAMERA_MOUNT_ENABLED 1
define HAL_MOUNT_ENABLED 1
define AP_MOUNT_BACKEND_DEFAULT_ENABLED 0
define HAL_MOUNT_SERVO_ENABLED 1
define QAUTOTUNE_ENABLED 1
define DEFAULT_NTF_LED_TYPES 257

1
libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc
View File

@ -102,7 +102,6 @@ define AP_LANDINGGEAR_ENABLED APM_BUILD_COPTER_OR_HELI
# Plane-specific defines; these defines are only used in the Plane
# directory, but are seen across the entire codebase:
define AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED 0
define QAUTOTUNE_ENABLED 0
# Copter-specific defines; these defines are only used in the Copter
# directory, but are seen across the entire codebase:

2
libraries/AP_HAL_ESP32/Scheduler.cpp
View File

@ -565,7 +565,9 @@ void IRAM_ATTR Scheduler::_main_thread(void *arg)
sched->delay_microseconds(250);
// run stats periodically
#ifdef SCHEDDEBUG
sched->print_stats();
#endif
sched->print_main_loop_rate();
if (ESP_OK != esp_task_wdt_reset()) {

20
libraries/AP_HAL_ESP32/Scheduler.h
View File

@ -85,16 +85,16 @@ public:
static const int IO_PRIO = 5;
static const int STORAGE_PRIO = 4;
static const int TIMER_SS = 4096;
static const int MAIN_SS = 8192;
static const int RCIN_SS = 4096;
static const int RCOUT_SS = 4096;
static const int WIFI_SS1 = 6192;
static const int WIFI_SS2 = 6192;
static const int UART_SS = 2048;
static const int DEVICE_SS = 4096;
static const int IO_SS = 4096;
static const int STORAGE_SS = 8192;
static const int TIMER_SS = 1024*3;
static const int MAIN_SS = 1024*5;
static const int RCIN_SS = 1024*3;
static const int RCOUT_SS = 1024*1.5;
static const int WIFI_SS1 = 1024*2.25;
static const int WIFI_SS2 = 1024*2.25;
static const int UART_SS = 1024*2.25;
static const int DEVICE_SS = 1024*4; // DEVICEBUS/s
static const int IO_SS = 1024*3.5; // APM_IO
static const int STORAGE_SS = 1024*2; // APM_STORAGE
private:
AP_HAL::HAL::Callbacks *callbacks;

9
libraries/AP_HAL_ESP32/boards/esp32empty.h
View File

@ -78,8 +78,10 @@
//SPI Devices
#define HAL_ESP32_SPI_DEVICES {}
//RCIN
#define HAL_ESP32_RCIN GPIO_NUM_36
//RMT pin number
#define HAL_ESP32_RMT_RX_PIN_NUMBER 4
//RCIN pin number - NOTE: disabled due to issue with legacy rmt library.
// #define HAL_ESP32_RCIN GPIO_NUM_36
//RCOUT
#define HAL_ESP32_RCOUT {GPIO_NUM_25, GPIO_NUM_27, GPIO_NUM_33, GPIO_NUM_32, GPIO_NUM_22, GPIO_NUM_21}
@ -124,9 +126,6 @@
//LED
#define DEFAULT_NTF_LED_TYPES Notify_LED_None
//RMT pin number
#define HAL_ESP32_RMT_RX_PIN_NUMBER 4
//SD CARD
// Do u want to use mmc or spi mode for the sd card, this is board specific,
// as mmc uses specific pins but is quicker,

4
libraries/AP_Logger/LogStructure.h
View File

@ -800,7 +800,7 @@ struct PACKED log_VER {
// @Field: txp: transmitted packet count
// @Field: rxp: received packet count
// @Field: rxdp: perceived number of packets we never received
// @Field: flags: compact representation of some stage of the channel
// @Field: flags: compact representation of some state of the channel
// @FieldBitmaskEnum: flags: GCS_MAVLINK::Flags
// @Field: ss: stream slowdown is the number of ms being added to each message to fit within bandwidth
// @Field: tf: times buffer was full when a message was going to be sent
@ -897,7 +897,7 @@ struct PACKED log_VER {
// @Field: Mem: Free memory available
// @Field: Load: System processor load
// @Field: InE: Internal error mask; which internal errors have been detected
// @FieldBitmaskEnum: IntE: AP_InternalError::error_t
// @FieldBitmaskEnum: InE: AP_InternalError::error_t
// @Field: ErrL: Internal error line number; last line number on which a internal error was detected
// @Field: ErC: Internal error count; how many internal errors have been detected
// @Field: SPIC: Number of SPI transactions processed

16
libraries/AP_Mission/AP_Mission.cpp
View File

@ -2446,16 +2446,22 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location &current_loc)
uint16_t landing_start_index = 0;
float min_distance = -1;
// This defines the maximum number of waypoints that will be searched, this limits the worst case runtime
uint16_t search_remaining = 1000;
// Go through mission and check each DO_RETURN_PATH_START
for (uint16_t i = 1; i < num_commands(); i++) {
Mission_Command tmp;
if (read_cmd_from_storage(i, tmp) && (tmp.id == MAV_CMD_DO_RETURN_PATH_START)) {
if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) {
uint16_t tmp_index;
float tmp_distance;
if (distance_to_mission_leg(i, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){
if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){
min_distance = tmp_distance;
landing_start_index = tmp_index;
}
if (search_remaining == 0) {
// Run out of time to search, stop and return the best so far
break;
}
}
}
@ -2626,7 +2632,7 @@ reset_do_jump_tracking:
// Approximate the distance travelled to return to the mission path. DO_JUMP commands are observed in look forward.
// Stop searching once reaching a landing or do-land-start
bool AP_Mission::distance_to_mission_leg(uint16_t start_index, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc)
bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc)
{
Location prev_loc;
Mission_Command temp_cmd;
@ -2642,7 +2648,7 @@ bool AP_Mission::distance_to_mission_leg(uint16_t start_index, float &rejoin_dis
// run through remainder of mission to approximate a distance to landing
uint16_t index = start_index;
for (uint8_t i=0; i<UINT8_MAX; i++) {
for (; search_remaining > 0; search_remaining--) {
// search until the end of the mission command list
for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) {
if (get_next_cmd(cmd_index, temp_cmd, true, false)) {

2
libraries/AP_Mission/AP_Mission.h
View File

@ -869,7 +869,7 @@ private:
// Approximate the distance traveled to return to the mission path. DO_JUMP commands are observed in look forward.
// Stop searching once reaching a landing or do-land-start
bool distance_to_mission_leg(uint16_t index, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc);
bool distance_to_mission_leg(uint16_t index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc);
// calculate the location of a resume cmd wp
bool calc_rewind_pos(Mission_Command& rewind_cmd);

40
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -13,6 +13,26 @@
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
{
// if reset source is not specified then use user defined velocity source
if (velResetSource == resetDataSource::DEFAULT) {
switch (frontend->sources.getVelXYSource()) {
case AP_NavEKF_Source::SourceXY::GPS:
velResetSource = resetDataSource::GPS;
break;
case AP_NavEKF_Source::SourceXY::BEACON:
velResetSource = resetDataSource::RNGBCN;
break;
case AP_NavEKF_Source::SourceXY::EXTNAV:
velResetSource = resetDataSource::EXTNAV;
break;
case AP_NavEKF_Source::SourceXY::NONE:
case AP_NavEKF_Source::SourceXY::OPTFLOW:
case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
// unhandled sources so stick with the default
break;
}
}
// Store the velocity before the reset so that we can record the reset delta
velResetNE.x = stateStruct.velocity.x;
velResetNE.y = stateStruct.velocity.y;
@ -73,6 +93,26 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
{
// if reset source is not specified thenn use the user defined position source
if (posResetSource == resetDataSource::DEFAULT) {
switch (frontend->sources.getPosXYSource()) {
case AP_NavEKF_Source::SourceXY::GPS:
posResetSource = resetDataSource::GPS;
break;
case AP_NavEKF_Source::SourceXY::BEACON:
posResetSource = resetDataSource::RNGBCN;
break;
case AP_NavEKF_Source::SourceXY::EXTNAV:
posResetSource = resetDataSource::EXTNAV;
break;
case AP_NavEKF_Source::SourceXY::NONE:
case AP_NavEKF_Source::SourceXY::OPTFLOW:
case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
// invalid sources so stick with the default
break;
}
}
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;

689
libraries/AP_Quicktune/AP_Quicktune.cpp Normal file
View File

@ -0,0 +1,689 @@
/*
C++ implementation of quicktune based on original lua script
*/
// quicktune is not performance sensitive, save flash
#pragma GCC optimize("Os")
#include "AP_Quicktune.h"
#if AP_QUICKTUNE_ENABLED
#define UPDATE_RATE_HZ 40
#define STAGE_DELAY 4000
#define PILOT_INPUT_DELAY 4000
#define YAW_FLTE_MAX 2.0
#define FLTD_MUL 0.5
#define FLTT_MUL 0.5
#define DEFAULT_SMAX 50.0
#define OPTIONS_TWO_POSITION (1<<0)
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Arming/AP_Arming.h>
#include <GCS_MAVLink/GCS.h>
const AP_Param::GroupInfo AP_Quicktune::var_info[] = {
// @Param: ENABLE
// @DisplayName: Quicktune enable
// @Description: Enable quicktune system
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Quicktune, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: AXES
// @DisplayName: Quicktune axes
// @Description: Axes to tune
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
// @User: Standard
AP_GROUPINFO("AXES", 2, AP_Quicktune, axes_enabled, 7),
// @Param: DOUBLE_TIME
// @DisplayName: Quicktune doubling time
// @Description: Time to double a tuning parameter. Raise this for a slower tune.
// @Range: 5 20
// @Units: s
// @User: Standard
AP_GROUPINFO("DOUBLE_TIME", 3, AP_Quicktune, double_time, 10),
// @Param: GAIN_MARGIN
// @DisplayName: Quicktune gain margin
// @Description: Reduction in gain after oscillation detected. Raise this number to get a more conservative tune
// @Range: 20 80
// @Units: %
// @User: Standard
AP_GROUPINFO("GAIN_MARGIN", 4, AP_Quicktune, gain_margin, 60),
// @Param: OSC_SMAX
// @DisplayName: Quicktune oscillation rate threshold
// @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune.
// @Range: 1 10
// @User: Standard
AP_GROUPINFO("OSC_SMAX", 5, AP_Quicktune, osc_smax, 4),
// @Param: YAW_P_MAX
// @DisplayName: Quicktune Yaw P max
// @Description: Maximum value for yaw P gain
// @Range: 0.1 3
// @User: Standard
AP_GROUPINFO("YAW_P_MAX", 6, AP_Quicktune, yaw_p_max, 0.5),
// @Param: YAW_D_MAX
// @DisplayName: Quicktune Yaw D max
// @Description: Maximum value for yaw D gain
// @Range: 0.001 1
// @User: Standard
AP_GROUPINFO("YAW_D_MAX", 7, AP_Quicktune, yaw_d_max, 0.01),
// @Param: RP_PI_RATIO
// @DisplayName: Quicktune roll/pitch PI ratio
// @Description: Ratio between P and I gains for roll and pitch. Raise this to get a lower I gain
// @Range: 1.0 2.0
// @User: Standard
AP_GROUPINFO("RP_PI_RATIO", 8, AP_Quicktune, rp_pi_ratio, 1.0),
// @Param: Y_PI_RATIO
// @DisplayName: Quicktune Yaw PI ratio
// @Description: Ratio between P and I gains for yaw. Raise this to get a lower I gain
// @Range: 1.0 20
// @User: Standard
AP_GROUPINFO("Y_PI_RATIO", 9, AP_Quicktune, y_pi_ratio, 10),
// @Param: AUTO_FILTER
// @DisplayName: Quicktune auto filter enable
// @Description: When enabled the PID filter settings are automatically set based on INS_GYRO_FILTER
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("AUTO_FILTER", 10, AP_Quicktune, auto_filter, 1),
// @Param: AUTO_SAVE
// @DisplayName: Quicktune auto save
// @Description: Number of seconds after completion of tune to auto-save. This is useful when using a 2 position switch for quicktune. Zero (the default value) disables automatic saving, and you will need to have a 3 position switch to save or use GCS auxilliary functions.
// @Units: s
// @User: Standard
AP_GROUPINFO("AUTO_SAVE", 11, AP_Quicktune, auto_save, 0),
// @Param: REDUCE_MAX
// @DisplayName: Quicktune maximum gain reduction
// @Description: This controls how much quicktune is allowed to lower gains from the original gains. If the vehicle already has a reasonable tune and is not oscillating then you can set this to zero to prevent gain reductions. The default of 20% is reasonable for most vehicles. Using a maximum gain reduction lowers the chance of an angle P oscillation happening if quicktune gets a false positive oscillation at a low gain, which can result in very low rate gains and a dangerous angle P oscillation.
// @Units: %
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("REDUCE_MAX", 12, AP_Quicktune, reduce_max, 20),
// @Param: OPTIONS
// @DisplayName: Quicktune options
// @Description: Additional options. When the Two Position Switch option is enabled then a high switch position will start the tune, low will disable the tune. you should also set a QUIK_AUTO_SAVE time so that you will be able to save the tune.
// @Bitmask: 0:UseTwoPositionSwitch
// @User: Standard
AP_GROUPINFO("OPTIONS", 13, AP_Quicktune, options, 0),
// @Param: ANGLE_MAX
// @DisplayName: maximum angle error for tune abort
// @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Quicktune: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QWIK_DOUBLE_TIME to do the tune more slowly.
// @Units: deg
// @User: Standard
AP_GROUPINFO("ANGLE_MAX", 14, AP_Quicktune, angle_max, 10),
AP_GROUPEND
};
// Call at loop rate
void AP_Quicktune::update(bool mode_supports_quicktune)
{
if (enable < 1) {
if (need_restore) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "QuickTune disabled");
abort_tune();
}
return;
}
const uint32_t now = AP_HAL::millis();
if (!mode_supports_quicktune) {
/*
user has switched to a non-quicktune mode. If we have
pending parameter changes then revert
*/
if (need_restore) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "QuickTune aborted");
}
abort_tune();
return;
}
if (need_restore) {
const float att_error = AC_AttitudeControl::get_singleton()->get_att_error_angle_deg();
if (angle_max > 0 && att_error > angle_max) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Quicktune: attitude error %.1fdeg - ABORTING", att_error);
abort_tune();
return;
}
}
if (have_pilot_input()) {
last_pilot_input_ms = now;
}
SwitchPos sw_pos_tune = SwitchPos::MID;
SwitchPos sw_pos_save = SwitchPos::HIGH;
if ((options & OPTIONS_TWO_POSITION) != 0) {
sw_pos_tune = SwitchPos::HIGH;
sw_pos_save = SwitchPos::NONE;
}
const auto &vehicle = *AP::vehicle();
if (sw_pos == sw_pos_tune &&
(!hal.util->get_soft_armed() || !vehicle.get_likely_flying())) {
if (now - last_warning_ms > 5000) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Must be flying to tune");
last_warning_ms = now;
}
return;
}
if (sw_pos == SwitchPos::LOW || !hal.util->get_soft_armed() || !vehicle.get_likely_flying()) {
// Abort, revert parameters
if (need_restore) {
need_restore = false;
restore_all_params();
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Reverted");
tune_done_time_ms = 0;
}
reset_axes_done();
return;
}
if (sw_pos == sw_pos_save) {
// Save all params
if (need_restore) {
need_restore = false;
save_all_params();
}
}
if (sw_pos != sw_pos_tune) {
return;
}
if (now - last_stage_change_ms < STAGE_DELAY) {
// Update slew gain
if (slew_parm != Param::END) {
const float P = get_param_value(slew_parm);
const AxisName axis = get_axis(slew_parm);
adjust_gain(slew_parm, P+slew_delta);
slew_steps = slew_steps - 1;
Write_QWIK(get_slew_rate(axis), P, slew_parm);
if (slew_steps == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f", get_param_name(slew_parm), P);
slew_parm = Param::END;
if (get_current_axis() == AxisName::DONE) {
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: DONE");
tune_done_time_ms = now;
}
}
}
return;
}
const AxisName axis = get_current_axis();
if (axis == AxisName::DONE) {
// Nothing left to do, check autosave time
if (tune_done_time_ms != 0 && auto_save > 0) {
if (now - tune_done_time_ms > (auto_save*1000)) {
need_restore = false;
save_all_params();
tune_done_time_ms = 0;
}
}
return;
}
if (!need_restore) {
need_restore = true;
// We are just starting tuning, get current values
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Starting tune");
// Get all params
for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
param_saved[p] = get_param_value(Param(p));
}
// Set up SMAX
const Param is[3] { Param::RLL_SMAX, Param::PIT_SMAX, Param::YAW_SMAX };
for (const auto p : is) {
const float smax = get_param_value(p);
if (smax <= 0) {
adjust_gain(p, DEFAULT_SMAX);
}
}
}
if (now - last_pilot_input_ms < PILOT_INPUT_DELAY) {
return;
}
if (!BIT_IS_SET(filters_done, uint8_t(axis))) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting %s tune", get_axis_name(axis));
setup_filters(axis);
}
const Param pname = get_pname(axis, current_stage);
const float pval = get_param_value(pname);
const float limit = gain_limit(pname);
const bool limited = (limit > 0.0 && pval >= limit);
const float srate = get_slew_rate(axis);
const bool oscillating = srate > osc_smax;
// Check if reached limit
if (limited || oscillating) {
float reduction = (100.0-gain_margin)*0.01;
if (!oscillating) {
reduction = 1.0;
}
float new_gain = pval * reduction;
if (limit > 0.0 && new_gain > limit) {
new_gain = limit;
}
float old_gain = param_saved[uint8_t(pname)];
if (new_gain < old_gain && (pname == Param::PIT_D || pname == Param::RLL_D)) {
// We are lowering a D gain from the original gain. Also
// lower the P gain by the same amount so that we don't
// trigger P oscillation. We don't drop P by more than a
// factor of 2
const float ratio = fmaxf(new_gain / old_gain, 0.5);
const uint8_t P_TO_D_OFS = uint8_t(Param::RLL_D) - uint8_t(Param::RLL_P);
const Param P_name = Param(uint8_t(pname)-P_TO_D_OFS); //from D to P
const float old_pval = get_param_value(P_name);;
const float new_pval = old_pval * ratio;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Adjusting %s %.3f -> %.3f", get_param_name(P_name), old_pval, new_pval);
adjust_gain_limited(P_name, new_pval);
}
// Set up slew gain
slew_parm = pname;
const float slew_target = limit_gain(pname, new_gain);
slew_steps = UPDATE_RATE_HZ/2;
slew_delta = (slew_target - get_param_value(pname)) / slew_steps;
Write_QWIK(srate, pval, pname);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Quicktune: %s done", get_param_name(pname));
advance_stage(axis);
last_stage_change_ms = now;
} else {
float new_gain = pval*get_gain_mul();
// cope with the gain starting at zero (some users with have a zero D gain)
new_gain = MAX(new_gain, 0.0001);
adjust_gain_limited(pname, new_gain);
Write_QWIK(srate, pval, pname);
if (now - last_gain_report_ms > 3000U) {
last_gain_report_ms = now;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f sr:%.2f", get_param_name(pname), new_gain, srate);
}
}
}
/*
abort the tune if it has started
*/
void AP_Quicktune::abort_tune()
{
if (need_restore) {
need_restore = false;
restore_all_params();
}
tune_done_time_ms = 0;
reset_axes_done();
sw_pos = SwitchPos::LOW;
}
void AP_Quicktune::update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag)
{
sw_pos = SwitchPos(ch_flag);
}
void AP_Quicktune::reset_axes_done()
{
axes_done = 0;
filters_done = 0;
current_stage = Stage::D;
}
void AP_Quicktune::setup_filters(AP_Quicktune::AxisName axis)
{
if (auto_filter <= 0) {
BIT_SET(filters_done, uint8_t(axis));
return;
}
AP_InertialSensor *imu = AP_InertialSensor::get_singleton();
if (imu == nullptr) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return;
}
const float gyro_filter = imu->get_gyro_filter_hz();
adjust_gain(get_pname(axis, Stage::FLTT), gyro_filter * FLTT_MUL);
adjust_gain(get_pname(axis, Stage::FLTD), gyro_filter * FLTD_MUL);
if (axis == AxisName::YAW) {
const float FLTE = get_param_value(Param::YAW_FLTE);
if (FLTE < 0.0 || FLTE > YAW_FLTE_MAX) {
adjust_gain(Param::YAW_FLTE, YAW_FLTE_MAX);
}
}
BIT_SET(filters_done, uint8_t(axis));
}
// Check for pilot input to pause tune
bool AP_Quicktune::have_pilot_input() const
{
auto &RC = rc();
const float roll = RC.get_roll_channel().norm_input_dz();
const float pitch = RC.get_pitch_channel().norm_input_dz();
const float yaw = RC.get_yaw_channel().norm_input_dz();
if (fabsf(roll) > 0 || fabsf(pitch) > 0 || fabsf(yaw) > 0) {
return true;
}
return false;
}
// Get the axis name we are working on, or DONE for all done
AP_Quicktune::AxisName AP_Quicktune::get_current_axis() const
{
for (int8_t i = 0; i < int8_t(AxisName::DONE); i++) {
if (BIT_IS_SET(axes_enabled, i) && !BIT_IS_SET(axes_done, i)) {
return AxisName(i);
}
}
return AxisName::DONE;
}
// get the AC_PID object for an axis
AC_PID *AP_Quicktune::get_axis_pid(AP_Quicktune::AxisName axis) const
{
auto &attitude_control = *AC_AttitudeControl::get_singleton();
switch(axis) {
case AxisName::RLL:
return &attitude_control.get_rate_roll_pid();
case AxisName::PIT:
return &attitude_control.get_rate_pitch_pid();
case AxisName::YAW:
return &attitude_control.get_rate_yaw_pid();
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
return nullptr;
}
// get slew rate parameter for an axis
float AP_Quicktune::get_slew_rate(AP_Quicktune::AxisName axis) const
{
auto *pid = get_axis_pid(axis);
if (pid == nullptr) {
return 0;
}
return pid->get_pid_info().slew_rate;
}
// Move to next stage of tune
void AP_Quicktune::advance_stage(AP_Quicktune::AxisName axis)
{
if (current_stage == Stage::D) {
current_stage = Stage::P;
} else {
BIT_SET(axes_done, uint8_t(axis));
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: %s done", get_axis_name(axis));
current_stage = Stage::D;
}
}
void AP_Quicktune::adjust_gain(AP_Quicktune::Param param, float value)
{
need_restore = true;
BIT_SET(param_changed, uint8_t(param));
set_param_value(param, value);
if (get_stage(param) == Stage::P) {
// Also change I gain
const uint8_t P_TO_I_OFS = uint8_t(Param::RLL_I) - uint8_t(Param::RLL_P);
const uint8_t P_TO_FF_OFS = uint8_t(Param::RLL_FF) - uint8_t(Param::RLL_P);
const Param iname = Param(uint8_t(param)+P_TO_I_OFS);
const Param ffname = Param(uint8_t(param)+P_TO_FF_OFS);
float FF = get_param_value(ffname);
if (FF > 0) {
// If we have any FF on an axis then we don't couple I to P,
// usually we want I = FF for a one second time constant for trim
return;
}
BIT_SET(param_changed, uint8_t(iname));
// Work out ratio of P to I that we want
float pi_ratio = rp_pi_ratio;
if (get_axis(param) == AxisName::YAW) {
pi_ratio = y_pi_ratio;
}
if (pi_ratio >= 1) {
set_param_value(iname, value/pi_ratio);
}
}
}
void AP_Quicktune::adjust_gain_limited(AP_Quicktune::Param param, float value)
{
adjust_gain(param, limit_gain(param, value));
}
float AP_Quicktune::limit_gain(AP_Quicktune::Param param, float value)
{
const float saved_value = param_saved[uint8_t(param)];
if (reduce_max >= 0 && reduce_max < 100 && saved_value > 0) {
// Check if we exceeded gain reduction
const float reduction_pct = 100.0 * (saved_value - value) / saved_value;
if (reduction_pct > reduce_max) {
const float new_value = saved_value * (100 - reduce_max) * 0.01;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Limiting %s %.3f -> %.3f", get_param_name(param), value, new_value);
value = new_value;
}
}
return value;
}
const char* AP_Quicktune::get_param_name(AP_Quicktune::Param param) const
{
switch (param)
{
case Param::RLL_P:
return "Roll P";
case Param::RLL_I:
return "Roll I";
case Param::RLL_D:
return "Roll D";
case Param::PIT_P:
return "Pitch P";
case Param::PIT_I:
return "Pitch I";
case Param::PIT_D:
return "Pitch D";
case Param::YAW_P:
return "Yaw P";
case Param::YAW_I:
return "Yaw I";
case Param::YAW_D:
return "Yaw D";
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return "UNK";
}
}
float AP_Quicktune::get_gain_mul() const
{
return expf(logf(2.0)/(UPDATE_RATE_HZ*MAX(1,double_time)));
}
void AP_Quicktune::restore_all_params()
{
for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
const auto param = Param(p);
if (BIT_IS_SET(param_changed, p)) {
set_param_value(param, param_saved[p]);
BIT_CLEAR(param_changed, p);
}
}
}
void AP_Quicktune::save_all_params()
{
for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
const auto param = Param(p);
set_and_save_param_value(param, get_param_value(param));
param_saved[p] = get_param_value(param);
BIT_CLEAR(param_changed, p);
}
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Saved");
}
AP_Quicktune::Param AP_Quicktune::get_pname(AP_Quicktune::AxisName axis, AP_Quicktune::Stage stage) const
{
const uint8_t axis_offset = uint8_t(axis) * param_per_axis;
switch (stage)
{
case Stage::P:
return Param(uint8_t(Param::RLL_P) + axis_offset);
case Stage::D:
return Param(uint8_t(Param::RLL_D) + axis_offset);
case Stage::FLTT:
return Param(uint8_t(Param::RLL_FLTT) + axis_offset);
case Stage::FLTD:
return Param(uint8_t(Param::RLL_FLTD) + axis_offset);
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return Param::END;
}
}
AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) const
{
if (param == Param::RLL_P || param == Param::PIT_P || param == Param::YAW_P) {
return Stage::P;
} else {
return Stage::END; //Means "anything but P gain"
}
}
AP_Float *AP_Quicktune::get_param_pointer(AP_Quicktune::Param param) const
{
const AxisName axis = get_axis(param);
auto *pid = get_axis_pid(axis);
if (pid == nullptr) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return nullptr;
}
const Param roll_param = Param(uint8_t(param) % param_per_axis);
switch (roll_param)
{
case Param::RLL_P:
return &pid->kP();
case Param::RLL_I:
return &pid->kI();
case Param::RLL_D:
return &pid->kD();
case Param::RLL_SMAX:
return &pid->slew_limit();
case Param::RLL_FLTT:
return &pid->filt_T_hz();
case Param::RLL_FLTD:
return &pid->filt_D_hz();
case Param::RLL_FLTE:
return &pid->filt_E_hz();
case Param::RLL_FF:
return &pid->ff();
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return nullptr;
}
}
float AP_Quicktune::get_param_value(AP_Quicktune::Param param) const
{
AP_Float *ptr = get_param_pointer(param);
if (ptr != nullptr) {
return ptr->get();
}
return 0.0;
}
void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value)
{
AP_Float *ptr = get_param_pointer(param);
if (ptr != nullptr) {
ptr->set(value);
}
}
void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value)
{
AP_Float *ptr = get_param_pointer(param);
if (ptr != nullptr) {
ptr->set_and_save(value);
}
}
AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) const
{
if (param >= Param::END) {
return AxisName::END;
}
return AxisName(uint8_t(param) / param_per_axis);
}
const char* AP_Quicktune::get_axis_name(AP_Quicktune::AxisName axis) const
{
switch (axis)
{
case AxisName::RLL:
return "Roll";
case AxisName::PIT:
return "Pitch";
case AxisName::YAW:
return "Yaw";
default:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return "UNK";
}
}
float AP_Quicktune::gain_limit(AP_Quicktune::Param param) const
{
if (get_axis(param) == AxisName::YAW) {
if (param == Param::YAW_P) {
return yaw_p_max;
}
if (param == Param::YAW_D) {
return yaw_d_max;
}
}
return 0.0;
}
// @LoggerMessage: QWIK
// @Description: Quicktune
// @Field: TimeUS: Time since system startup
// @Field: ParamNo: number of parameter being tuned
// @Field: SRate: slew rate
// @Field: Gain: test gain for current axis and PID element
// @Field: Param: name of parameter being being tuned
void AP_Quicktune::Write_QWIK(float srate, float gain, AP_Quicktune::Param param)
{
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("QWIK","TimeUS,ParamNo,SRate,Gain,Param", "s#---", "-----", "QBffN",
AP_HAL::micros64(),
unsigned(param),
srate,
gain,
get_param_name(param));
#endif
}
#endif //AP_QUICKTUNE_ENABLED

167
libraries/AP_Quicktune/AP_Quicktune.h Normal file
View File

@ -0,0 +1,167 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_QUICKTUNE_ENABLED
#define AP_QUICKTUNE_ENABLED 1 // NOTE: may be disabled by vehicle header
#endif
#if AP_QUICKTUNE_ENABLED
#include <AC_AttitudeControl/AC_AttitudeControl.h>
#include <AP_Param/AP_Param.h>
#include <RC_Channel/RC_Channel.h>
class AP_Quicktune {
public:
AP_Quicktune()
{
AP_Param::setup_object_defaults(this, var_info);
}
// Empty destructor to suppress compiler warning
virtual ~AP_Quicktune() {}
/* Do not allow copies */
CLASS_NO_COPY(AP_Quicktune);
// Parameter block
static const struct AP_Param::GroupInfo var_info[];
void update(bool mode_supports_quicktune);
void update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag);
private:
// Parameters
AP_Int8 enable;
AP_Int8 axes_enabled;
AP_Float double_time;
AP_Float gain_margin;
AP_Float osc_smax;
AP_Float yaw_p_max;
AP_Float yaw_d_max;
AP_Float rp_pi_ratio;
AP_Float y_pi_ratio;
AP_Int8 auto_filter;
AP_Float auto_save;
AP_Float reduce_max;
AP_Int16 options;
AP_Int8 angle_max;
// Low, Mid and High must be in the same positions as they are in RC_Channel::AuxSwitchPos
enum class SwitchPos : uint8_t {
LOW,
MID,
HIGH,
NONE,
};
enum class AxisName : uint8_t {
RLL = 0,
PIT,
YAW,
DONE,
END,
};
/*
note! we rely on the enum being in the same order between axes
*/
enum class Param : uint8_t {
RLL_P = 0,
RLL_I,
RLL_D,
RLL_SMAX,
RLL_FLTT,
RLL_FLTD,
RLL_FLTE,
RLL_FF,
PIT_P,
PIT_I,
PIT_D,
PIT_SMAX,
PIT_FLTT,
PIT_FLTD,
PIT_FLTE,
PIT_FF,
YAW_P,
YAW_I,
YAW_D,
YAW_SMAX,
YAW_FLTT,
YAW_FLTD,
YAW_FLTE,
YAW_FF,
END,
};
static const uint8_t param_per_axis = uint8_t(Param::PIT_P) - uint8_t(Param::RLL_P);
static_assert(uint8_t(Param::END) == 3*param_per_axis, "AP_Quicktune Param error");
// Also the gains
enum class Stage : uint8_t {
D,
P,
DONE,
I,
FF,
SMAX,
FLTT,
FLTD,
FLTE,
END,
};
// Time keeping
uint32_t last_stage_change_ms;
uint32_t last_gain_report_ms;
uint32_t last_pilot_input_ms;
uint32_t last_warning_ms;
uint32_t tune_done_time_ms;
// Bitmasks
uint32_t axes_done;
uint32_t filters_done;
uint32_t param_changed; //Bitmask of changed parameters
Stage current_stage = Stage::D;
Param slew_parm = Param::END;
uint8_t slew_steps;
float slew_delta;
SwitchPos sw_pos; //Switch pos to be set by aux func
bool need_restore;
float param_saved[uint8_t(Param::END)]; //Saved values of the parameters
void reset_axes_done();
void setup_filters(AxisName axis);
bool have_pilot_input() const;
AxisName get_current_axis() const;
float get_slew_rate(AxisName axis) const;
void advance_stage(AxisName axis);
void adjust_gain(Param param, float value);
void adjust_gain_limited(Param param, float value);
float get_gain_mul() const;
void restore_all_params();
void save_all_params();
Param get_pname(AxisName axis, Stage stage) const;
AP_Float *get_param_pointer(Param param) const;
float get_param_value(Param param) const;
void set_param_value(Param param, float value);
void set_and_save_param_value(Param param, float value);
float gain_limit(Param param) const;
AxisName get_axis(Param param) const;
float limit_gain(Param param, float value);
const char* get_param_name(Param param) const;
Stage get_stage(Param param) const;
const char* get_axis_name(AxisName axis) const;
AC_PID *get_axis_pid(AxisName axis) const;
void Write_QWIK(float SRate, float Gain, Param param);
void abort_tune(void);
};
#endif // AP_QUICKTUNE_ENABLED

5
libraries/AP_RTC/AP_RTC.cpp
View File

@ -114,9 +114,8 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
// @LoggerMessage: RTC
// @Description: Information about RTC clock resets
// @Field: TimeUS: Time since system startup
// @Field: old: old time
// @Field: new: new time
// @Field: in: new argument time
// @Field: old_utc: old time
// @Field: new_utc: new time
AP::logger().WriteStreaming(
"RTC",
"TimeUS,old_utc,new_utc",

1
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -14,6 +14,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Rangefinder type
// @Description: Type of connected rangefinder
// @SortValues: AlphabeticalZeroAtTop
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),

19
libraries/AP_Scripting/docs/docs.lua
View File

@ -2826,6 +2826,25 @@ function vehicle:is_landing() end
---@return boolean -- true on success
function vehicle:set_crosstrack_start(new_start_location) end
-- Register a custom mode. This behaves like guided mode but will report with a custom number and name
---@param number integer -- mode number to use, should be over 100
---@param full_name string -- Full mode name
---@param short_name string -- Short mode name upto 4 characters
---@return AP_Vehicle__custom_mode_state_ud|nil -- Returns custom mode state which allows customisation of behavior, nil if mode register fails
function vehicle:register_custom_mode(number, full_name, short_name) end
-- Custom mode state, allows customisation of mode behavior
---@class (exact) AP_Vehicle__custom_mode_state_ud
local AP_Vehicle__custom_mode_state_ud = {}
-- get allow_entry, if true the vehicle is allowed to enter this custom mode
---@return boolean
function AP_Vehicle__custom_mode_state_ud:allow_entry() end
-- set allow_entry, if true the vehicle is allowed to enter this custom mode
---@param value boolean
function AP_Vehicle__custom_mode_state_ud:allow_entry(value) end
-- desc
onvif = {}

246
libraries/AP_Scripting/examples/Flip_Mode.lua Normal file
View File

@ -0,0 +1,246 @@
-- This script replicates the behavior of flip mode
local MODE_NUMBER = 100
-- Register flip as mode 100
local FLIP_MODE_STATE = assert(vehicle:register_custom_mode(MODE_NUMBER, "Flip 2", "FLI2"))
-- Get input channels
local THROTTLE_CHAN = math.floor(assert(param:get("RCMAP_THROTTLE")))
local pilot_throttle = assert(rc:get_channel(THROTTLE_CHAN))
local pilot_pitch = assert(rc:get_channel(assert(param:get("RCMAP_PITCH"))))
local pilot_roll = assert(rc:get_channel(assert(param:get("RCMAP_ROLL"))))
local HOVER_THROTTLE = Parameter("MOT_THST_HOVER")
local THROTTLE_MIN = Parameter("RC" .. THROTTLE_CHAN .. "_MIN")
local THROTTLE_MAX = Parameter("RC" .. THROTTLE_CHAN .. "_MAX")
local THROTTLE_DZ = Parameter("RC" .. THROTTLE_CHAN .. "_DZ")
-- Replication of the copter function
local function get_throttle_mid()
local r_in = (THROTTLE_MIN:get() + THROTTLE_MAX:get()) * 0.5
local radio_trim_low = THROTTLE_MIN:get() + THROTTLE_DZ:get()
return 1000.0 * ((r_in - radio_trim_low) / (THROTTLE_MAX:get() - radio_trim_low))
end
-- Replication of the copter function
local function get_pilot_desired_throttle()
local thr_mid = HOVER_THROTTLE:get()
local throttle_control = (pilot_throttle:norm_input_ignore_trim() + 1.0) * (1000.0 / 2.0)
local mid_stick = get_throttle_mid()
local throttle_in
if throttle_control < mid_stick then
throttle_in = throttle_control * 0.5 / mid_stick
else
throttle_in = 0.5 + ((throttle_control - mid_stick) * 0.5 / (1000.0 - mid_stick))
end
local expo = -(thr_mid - 0.5) / 0.375
expo = math.max(expo, -0.5)
expo = math.min(expo, 1.0)
return throttle_in * (1.0-expo) + expo*throttle_in*throttle_in*throttle_in
end
-- Used to check for mode changes
local last_mode_number
local MODES = {
STABILIZE = 0,
ALT_HOLD = 2,
}
-- Check if we should be allowed to enter flip mode
local function allow_enter(previous_mode)
-- Only allow flip from stabilize and alt hold
if (previous_mode ~= MODES.STABILIZE) and (previous_mode ~= MODES.ALT_HOLD) then
return false
end
-- Must be armed and flying
if (not arming:is_armed()) or (not vehicle:get_likely_flying()) then
return false
end
return true
end
local FLIP_STATE = {
START = 0,
ROLL = 1,
PITCH_A = 2,
PITCH_B = 3,
RECOVER = 4,
}
local state
local start_ms
local roll_dir
local pitch_dir
local start_attitude = {
roll = 0,
pitch = 0,
yaw = 0,
}
-- Init on first call
local previous_mode
local function init()
-- Record the previous mode, this is returned to on completion of the flip
previous_mode = last_mode_number
-- Return to previous mode immediately if flip cannot be performed
if not allow_enter(previous_mode) then
vehicle:set_mode(previous_mode)
return
end
state = FLIP_STATE.START
start_ms = millis()
roll_dir = 0.0
pitch_dir = 0.0
-- choose direction based on pilot's roll and pitch sticks
if pilot_pitch:norm_input_dz() > 0.1 then
pitch_dir = 1.0
elseif pilot_pitch:norm_input_dz() < -0.1 then
pitch_dir = -1.0
elseif pilot_roll:norm_input_dz() >= 0 then
roll_dir = 1.0
else
roll_dir = -1.0
end
-- Record start attitude to be used in recovery stage
start_attitude.roll = math.deg(ahrs:get_roll())
start_attitude.pitch = math.deg(ahrs:get_pitch())
start_attitude.yaw = math.deg(ahrs:get_yaw())
end
local FLIP_THR_INC = 0.2
local FLIP_THR_DEC = 0.24
local FLIP_ROTATION_RATE = 400
local function run()
local NOW = millis()
-- Disarmed, pilot input or timeout then return to previous mode
local PILOT_INPUT = (math.abs(pilot_roll:norm_input_dz()) > 0.85) or (math.abs(pilot_pitch:norm_input_dz()) > 0.85)
if (not arming:is_armed()) or PILOT_INPUT or ((NOW - start_ms) > 2500) then
vehicle:set_mode(previous_mode)
return
end
local roll_deg = math.deg(ahrs:get_roll())
local pitch_deg = math.deg(ahrs:get_pitch())
if state == FLIP_STATE.RECOVER then
-- Target original attitude with 0 climb rate
vehicle:set_target_angle_and_climbrate(start_attitude.roll, start_attitude.pitch, start_attitude.yaw, 0.0, false, 0.0)
-- See if we have returned to the desired angle
local recovery_angle = math.abs((roll_deg - start_attitude.roll) * roll_dir) + math.abs((pitch_deg - start_attitude.pitch) * pitch_dir)
if recovery_angle < 5.0 then
-- Complete, return to original mode
vehicle:set_mode(previous_mode)
end
return
end
local throttle_out = get_pilot_desired_throttle()
local flip_angle = roll_deg * roll_dir + pitch_deg * pitch_dir
if state == FLIP_STATE.START then
-- Increase throttle
throttle_out = math.min(throttle_out + FLIP_THR_INC, 1.0)
-- Check for next stage
if flip_angle >= 45.0 then
if roll_dir ~= 0 then
-- Rolling flip
state = FLIP_STATE.ROLL
else
-- Pitching flip
state = FLIP_STATE.PITCH_A
end
end
elseif state == FLIP_STATE.ROLL then
-- decrease throttle
throttle_out = math.max(throttle_out - FLIP_THR_DEC, 0.0)
-- beyond 90deg move on to recovery
if (flip_angle < 45.0) and (flip_angle > -90.0) then
state = FLIP_STATE.RECOVER
end
elseif state == FLIP_STATE.PITCH_A then
-- decrease throttle
throttle_out = math.max(throttle_out - FLIP_THR_DEC, 0.0)
-- check roll for inversion
if (math.abs(roll_deg) > 90.0) and (flip_angle > 45.0) then
state = FLIP_STATE.PITCH_B
end
elseif state == FLIP_STATE.PITCH_B then
-- decrease throttle
throttle_out = math.max(throttle_out - FLIP_THR_DEC, 0.0)
-- check roll for un-inversion
if (math.abs(roll_deg) < 90.0) and (flip_angle > -45.0) then
state = FLIP_STATE.RECOVER
end
end
-- Send rate and throttle command to vehicle
local roll_rate = FLIP_ROTATION_RATE * roll_dir
local pitch_rate = FLIP_ROTATION_RATE * pitch_dir
vehicle:set_target_rate_and_throttle(roll_rate, pitch_rate, 0.0, throttle_out)
end
local function exit()
-- Nothing to do here
end
local function update()
-- Only allow entry into flip mode if armed and flying
local armed = arming:is_armed()
local flying = vehicle:get_likely_flying()
FLIP_MODE_STATE:allow_entry(armed and flying)
local mode = vehicle:get_mode()
if mode == MODE_NUMBER then
if last_mode_number ~= MODE_NUMBER then
-- Fist call after entering
init()
else
-- Runtime function
run()
end
elseif last_mode_number == MODE_NUMBER then
-- Exit mode
exit()
end
last_mode_number = mode
-- Run at 100Hz
return update, 10
end
return update()

219
libraries/AP_Scripting/examples/plane-callout-alt.lua Normal file
View File

@ -0,0 +1,219 @@
--[[
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
--]]
SCRIPT_VERSION = "4.6.0-003"
SCRIPT_NAME = "Plane Altitude Callouts"
SCRIPT_NAME_SHORT = "Callout"
-- This script calls out altitude if the user is near (WARN) or above (MAX)
-- Allows for units to be feet even if you do everything else in metric because the laws typically specify 400 feet for UAV/RPAS in most countries
-- all of the settings are stored in parameters:
-- CALLOUT_ALT_UNITS 1 = metric, 2 (default) = imperial
-- CALLOUT_ALT_MAX max allowed altitude (its still a message there is no action)
-- CALLOUT_ALT_STEP callout (via GC message) when altitude changes by this amount or more
-- CALLOUT_ALT_CALL seconds between callout of flying altitude
-- CALLOUT_ALT_WARN seconds between callouts that you are less than ALT_STEP below ALT_MAX
-- CALLOUT_ALT_HIGH seconds between callouts that you have exceeded ALT_MAX
REFRESH_RATE = 1000 --check every 1 second
MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
local PARAM_TABLE_KEY = 102
local PARAM_TABLE_PREFIX = "ZPC_"
-- add a parameter and bind it to a variable
local function bind_add_param(name, idx, default_value)
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
return Parameter(PARAM_TABLE_PREFIX .. name)
end
-- setup follow mode specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table')
-- Add these ZPC_ parameters specifically for this script
--[[
// @Param: ZPC_ALT_UNITS
// @DisplayName: Plane Callouts Units - defaults to feet
// @Description: 1: Metric/meters or 2: Imperial/feet
// @User: Standard
--]]
ZPC_ALT_UNITS = bind_add_param('ALT_UNITS', 1, 2)
--[[
// @Param: ZPC_ALT_MAX
// @DisplayName: Plane Callouts max altitude
// @Description: Maximum altitude in ALT_UNITS
// @Range: 0 30
// @Units: seconds
--]]
ZPC_ALT_MAX = bind_add_param("ALT_MAX", 2, 400)
--[[
// @Param: ZPC_ALT_STEP
// @DisplayName: Plane Callouts altitude steps
// @Description: Altitude steps for callouts in ALT_UNITS
// @Range: 0 100
// @Units: ALT_UNIT
--]]
ZPC_ALT_STEP = bind_add_param("ALT_STEP", 3, 25)
--[[
// @Param: ZPC_ALT_CALL
// @DisplayName: Plane Callouts frequency
// @Description: How often to callout altitude when flying normally
// @Range: 0 30
// @Units: seconds
--]]
ZPC_ALT_CALL = bind_add_param("ALT_CALL", 4, 25)
--[[
// @Param: ZPC_ALT_WARN
// @DisplayName: Plane altitude warning frequency
// @Description: How often to nag about almost hitting MAX
// @Range: 0 30
// @Units: seconds
--]]
ZPC_ALT_WARN = bind_add_param("ALT_WARN", 5, 25)
--[[
// @Param: ZPC_ALT_HIGH
// @DisplayName: Plane altitude max altitude callout frequency
// @Description: How often to nag about exceeding MAX
// @Range: 0 30
// @Units: seconds
--]]
ZPC_ALT_HIGH = bind_add_param("ALT_HIGH", 6, 10)
local alt_units = ZPC_ALT_UNITS:get() or 2 -- default to feet because "thats what it is"
local alt_max = ZPC_ALT_MAX:get() or 400 -- most places this is the legal limit, if you have a different limit, change this
local alt_step = ZPC_ALT_STEP:get() or 25
local alt_call_sec = ZPC_ALT_CALL:get() or 25
local alt_warn_sec = ZPC_ALT_WARN:get() or 25
local alt_high_sec = ZPC_ALT_HIGH:get() or 10
local alt_last = 0
local alt_warn = alt_max - alt_step
local unit = "meters"
if (alt_units == 2) then
unit = "feet"
end
local altitude_max = string.format("%i %s", math.floor(alt_max+0.5), unit )
local time_last_warn_s = millis() / 1000
local time_last_max_s = millis() / 1000
local time_last_update_s = millis() / 1000
local alt_max_exceeded = false
local alt_warn_exceeded = false
local function update() -- this is the loop which periodically runs
local current_time_s = millis() / 1000
-- setting the height/altitude variables like this means all the code below works without change for either metric or Imperial units
local terrain_height = terrain:height_above_terrain(true)
-- if terrain height is not available use height above home
if terrain_height == nil then
-- override terrain height with home height (TODO: parameterize this)
local pos = ahrs:get_relative_position_NED_home()
if pos == nil then
return
else
terrain_height = -pos:z()
end
end
--- allow these parameters to be changed at runtime.
alt_units = ZPC_ALT_UNITS:get() or 2
alt_max = ZPC_ALT_MAX:get() or 400 -- most places this is the legal limit, if you have a different limit, change this
alt_step = ZPC_ALT_STEP:get() or 25
alt_call_sec = ZPC_ALT_CALL:get() or 25
alt_warn_sec = ZPC_ALT_WARN:get() or 25
alt_high_sec = ZPC_ALT_HIGH:get() or 10
if (alt_units == 2) then
unit = "feet"
else
unit = "meters"
end
if (alt_units == 2) then
terrain_height = terrain_height * 3.28084
end
local altitude = string.format("%i %s", math.floor(terrain_height+0.5), unit )
if terrain_height > alt_max then
if (time_last_max_s < current_time_s - alt_high_sec) then
gcs:send_text(MAV_SEVERITY.ERROR, string.format("Altitude %s too high max %s", altitude, altitude_max ))
time_last_max_s = current_time_s
alt_max_exceeded = true
end
elseif terrain_height > alt_warn then
if (time_last_warn_s < current_time_s - alt_warn_sec) then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("Warning altitude is %s", altitude ))
time_last_warn_s = current_time_s
alt_warn_exceeded = true
end
else
-- we are fine now, but maybe we were not fine before.
-- So if we previously displayed altitude warn/error messages, let the pilot know we are now fine
if(alt_max_exceeded or alt_warn_exceeded) then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("Altitude %s is Ok", altitude ))
else
-- nothing else important happened, so see if our altitude has gone up or down by more than ALT_STEP
-- in which case we call it out
if (time_last_update_s < current_time_s - alt_call_sec) then
local alt_diff = (terrain_height - alt_last)
if (math.abs(alt_diff) > alt_step) then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("Altitude is %s", altitude ))
alt_last = math.floor(terrain_height / alt_step + 0.5) * alt_step
end
time_last_update_s = current_time_s
end
end
alt_max_exceeded = false
alt_warn_exceeded = false
end
end
local displayed_banner = false
-- wrapper around update(). This calls update() at 1Hz,
-- and if update faults then an error is displayed, but the script is not
-- stopped
local function protected_wrapper()
if not displayed_banner then
gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded callouts in %s", SCRIPT_NAME, SCRIPT_VERSION, unit) )
displayed_banner = true
end
if not arming:is_armed() then return protected_wrapper, REFRESH_RATE * 10 end
local success, err = pcall(update)
if not success then
gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "Internal Error: " .. err)
-- when we fault we run the update function again after 1s, slowing it
-- down a bit so we don't flood the console with errors
return protected_wrapper, 1000
end
return protected_wrapper, REFRESH_RATE
end
-- start running update loop - wait 20 seconds before starting up
return protected_wrapper, 20000

4
libraries/AP_Scripting/generator/description/bindings.desc
View File

@ -309,6 +309,9 @@ singleton AP_ONVIF method set_absolutemove boolean float'skip_check float'skip_c
singleton AP_ONVIF method get_pan_tilt_limit_min Vector2f
singleton AP_ONVIF method get_pan_tilt_limit_max Vector2f
ap_object AP_Vehicle::custom_mode_state depends (!defined(HAL_BUILD_AP_PERIPH))
ap_object AP_Vehicle::custom_mode_state field allow_entry boolean read write
include AP_Vehicle/AP_Vehicle.h
singleton AP_Vehicle depends (!defined(HAL_BUILD_AP_PERIPH))
singleton AP_Vehicle rename vehicle
@ -352,6 +355,7 @@ singleton AP_Vehicle method reboot void boolean
singleton AP_Vehicle method is_landing boolean
singleton AP_Vehicle method is_taking_off boolean
singleton AP_Vehicle method set_crosstrack_start boolean Location
singleton AP_Vehicle method register_custom_mode AP_Vehicle::custom_mode_state uint8_t'skip_check string string
include AP_SerialLED/AP_SerialLED.h

29
libraries/AP_Scripting/generator/src/main.c
View File

@ -1222,8 +1222,11 @@ void handle_ap_object(void) {
} else if (strcmp(type, keyword_manual) == 0) {
handle_manual(node, ALIAS_TYPE_MANUAL);
} else if (strcmp(type, keyword_field) == 0) {
handle_userdata_field(node);
} else {
error(ERROR_SINGLETON, "AP_Objects only support renames, methods, semaphore or manual keywords (got %s)", type);
error(ERROR_SINGLETON, "AP_Objects only support renames, methods, field, semaphore or manual keywords (got %s)", type);
}
// check that we didn't just add 2 singleton flags
@ -1830,6 +1833,28 @@ void emit_singleton_fields() {
}
}
void emit_ap_object_field(const struct userdata *data, const struct userdata_field *field) {
fprintf(source, "static int %s_%s(lua_State *L) {\n", data->sanatized_name, field->name);
fprintf(source, " %s *ud = *check_%s(L, 1);\n", data->name, data->sanatized_name);
emit_field(field, "ud", "->");
}
void emit_ap_object_fields() {
struct userdata * node = parsed_ap_objects;
while(node) {
struct userdata_field *field = node->fields;
if (field) {
start_dependency(source, node->dependency);
while(field) {
emit_ap_object_field(node, field);
field = field->next;
}
end_dependency(source, node->dependency);
}
node = node->next;
}
}
// emit refences functions for a call, return the number of arduments added
int emit_references(const struct argument *arg, const char * tab) {
int arg_index = NULLABLE_ARG_COUNT_BASE + 2;
@ -3162,7 +3187,7 @@ int main(int argc, char **argv) {
emit_methods(parsed_userdata);
emit_index(parsed_userdata);
emit_ap_object_fields();
emit_methods(parsed_ap_objects);
emit_index(parsed_ap_objects);

8
libraries/AP_Soaring/AP_Soaring.h
View File

@ -23,10 +23,10 @@
#include "Variometer.h"
#include "SpeedToFly.h"
#define INITIAL_THERMAL_RADIUS 80.0
#define INITIAL_STRENGTH_COVARIANCE 0.0049
#define INITIAL_RADIUS_COVARIANCE 400.0
#define INITIAL_POSITION_COVARIANCE 400.0
static constexpr float INITIAL_THERMAL_RADIUS = 80.0;
static constexpr float INITIAL_STRENGTH_COVARIANCE = 0.0049;
static constexpr float INITIAL_RADIUS_COVARIANCE = 400.0;
static constexpr float INITIAL_POSITION_COVARIANCE = 400.0;
class SoaringController {

29
libraries/AP_TECS/AP_TECS.cpp
View File

@ -1124,7 +1124,6 @@ void AP_TECS::_initialise_states(float hgt_afe)
_integKE = 0.0f;
_last_throttle_dem = aparm.throttle_cruise * 0.01f;
_last_pitch_dem = _ahrs.get_pitch();
_hgt_afe = hgt_afe;
_hgt_dem_in_prev = hgt_afe;
_hgt_dem_lpf = hgt_afe;
_hgt_dem_rate_ltd = hgt_afe;
@ -1501,4 +1500,32 @@ void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
// don't allow max pitch to go below min pitch
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
}
void AP_TECS::offset_altitude(const float alt_offset)
{
// Convention: When alt_offset is positive it means that the altitude of
// home has increased. Thus, the relative altitude of the vehicle has
// decreased.
//
// Assumption: This method is called more often and before
// `update_pitch_throttle()`. This is necessary to ensure that new height
// demands which incorporate the home change are compatible with the
// (now updated) internal height state.
_flare_hgt_dem_ideal -= alt_offset;
_flare_hgt_dem_adj -= alt_offset;
_hgt_at_start_of_flare -= alt_offset;
_hgt_dem_in_prev -= alt_offset;
_hgt_dem_lpf -= alt_offset;
_hgt_dem_rate_ltd -= alt_offset;
_hgt_dem_prev -= alt_offset;
_height_filter.height -= alt_offset;
// The following variables are updated anew in every call of
// `update_pitch_throttle()`. There's no need to update those.
// _hgt_dem
// _hgt_dem_in_raw
// _hgt_dem_in
// Energies
}

3
libraries/AP_TECS/AP_TECS.h
View File

@ -154,6 +154,9 @@ public:
_need_reset = true;
}
// Apply an altitude offset, to compensate for changes in home alt.
void offset_altitude(const float alt_offset);
// this supports the TECS_* user settable parameters
static const struct AP_Param::GroupInfo var_info[];

18
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -154,19 +154,17 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
}
// hXY are the heights of the 4 surrounding grid points
int16_t h00, h01, h10, h11;
h00 = grid.height[info.idx_x+0][info.idx_y+0];
h01 = grid.height[info.idx_x+0][info.idx_y+1];
h10 = grid.height[info.idx_x+1][info.idx_y+0];
h11 = grid.height[info.idx_x+1][info.idx_y+1];
const auto h00 = grid.height[info.idx_x+0][info.idx_y+0];
const auto h01 = grid.height[info.idx_x+0][info.idx_y+1];
const auto h10 = grid.height[info.idx_x+1][info.idx_y+0];
const auto h11 = grid.height[info.idx_x+1][info.idx_y+1];
// do a simple dual linear interpolation. We could do something
// fancier, but it probably isn't worth it as long as the
// grid_spacing is kept small enough
float avg1 = (1.0f-info.frac_x) * h00 + info.frac_x * h10;
float avg2 = (1.0f-info.frac_x) * h01 + info.frac_x * h11;
float avg = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;
const float avg1 = (1.0f-info.frac_x) * h00 + info.frac_x * h10;
const float avg2 = (1.0f-info.frac_x) * h01 + info.frac_x * h11;
const float avg = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;
height = avg;
@ -567,7 +565,7 @@ void AP_Terrain::update_reference_offset(void)
if (!reference_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
return;
}
float adjustment = alt_cm*0.01 - height;
const float adjustment = alt_cm*0.01 - height;
reference_offset = constrain_float(adjustment, -offset_max, offset_max);
if (fabsf(adjustment) > offset_max.get()+0.5) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Terrain: clamping offset %.0f to %.0f",

9
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -172,6 +172,8 @@ public:
virtual bool is_crashed() const;
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
// Method to takeoff for use by external control
virtual bool start_takeoff(const float alt) { return false; }
// Method to control vehicle position for use by external control
virtual bool set_target_location(const Location& target_loc) { return false; }
#endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
@ -179,7 +181,6 @@ public:
/*
methods to control vehicle for use by scripting
*/
virtual bool start_takeoff(float alt) { return false; }
virtual 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) { return false; }
virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; }
virtual 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) { return false; }
@ -246,6 +247,12 @@ public:
// returns true on success and control_value is set to a value in the range -1 to +1
virtual bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) { return false; }
// Register a custom mode with given number and names, return a structure which the script can edit
struct custom_mode_state {
bool allow_entry;
};
virtual custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) { return nullptr; }
#endif // AP_SCRIPTING_ENABLED
// returns true if vehicle is in the process of landing

10
libraries/GCS_MAVLink/GCS.h
View File

@ -1139,6 +1139,7 @@ private:
uint8_t next_index;
} available_modes;
bool send_available_modes();
bool send_available_mode_monitor();
};
@ -1301,6 +1302,11 @@ public:
MAV_RESULT lua_command_int_packet(const mavlink_command_int_t &packet);
#endif
// Sequence number should be incremented when available modes changes
// Sent in AVAILABLE_MODES_MONITOR msg
uint8_t get_available_modes_sequence() const { return available_modes_sequence; }
void available_modes_changed() { available_modes_sequence += 1; }
protected:
virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
@ -1378,6 +1384,10 @@ private:
// GCS::update_send is called so we don't starve later links of
// time in which they are permitted to send messages.
uint8_t first_backend_to_send;
// Sequence number should be incremented when available modes changes
// Sent in AVAILABLE_MODES_MONITOR msg
uint8_t available_modes_sequence;
};
GCS &gcs();

21
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1153,6 +1153,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED},
#endif
{ MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES},
{ MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, MSG_AVAILABLE_MODES_MONITOR},
};
for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@ -3210,6 +3211,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int
available_modes.should_send = true;
available_modes.next_index = 1;
available_modes.requested_index = (uint8_t)packet.param2;
// After the first request sequnece is streamed in the AVAILABLE_MODES_MONITOR message
// This allows the GCS to re-request modes if there is a change
set_ap_message_interval(MSG_AVAILABLE_MODES_MONITOR, 5000);
break;
default:
@ -6137,6 +6142,18 @@ bool GCS_MAVLINK::send_available_modes()
return true;
}
bool GCS_MAVLINK::send_available_mode_monitor()
{
CHECK_PAYLOAD_SIZE(AVAILABLE_MODES_MONITOR);
mavlink_msg_available_modes_monitor_send(
chan,
gcs().get_available_modes_sequence()
);
return true;
}
bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
bool ret = true;
@ -6571,6 +6588,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
ret = send_available_modes();
break;
case MSG_AVAILABLE_MODES_MONITOR:
ret = send_available_mode_monitor();
break;
default:
// try_send_message must always at some stage return true for
// a message, or we will attempt to infinitely retry the

13
libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp
View File

@ -19,7 +19,7 @@ bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq,
if (fence == nullptr) {
return false;
}
const uint8_t num_stored_items = fence->polyfence().num_stored_items();
const auto num_stored_items = fence->polyfence().num_stored_items();
if (seq > num_stored_items) {
return false;
}
@ -66,6 +66,7 @@ bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq,
ret_packet.x = fenceitem.loc.x;
ret_packet.y = fenceitem.loc.y;
ret_packet.z = 0;
ret_packet.mission_type = MAV_MISSION_TYPE_FENCE;
return true;
}
@ -75,7 +76,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link,
const mavlink_mission_request_int_t &packet,
mavlink_mission_item_int_t &ret_packet)
{
const uint8_t num_stored_items = _fence.polyfence().num_stored_items();
const auto num_stored_items = _fence.polyfence().num_stored_items();
if (packet.seq > num_stored_items) {
return MAV_MISSION_INVALID_SEQUENCE;
}
@ -109,10 +110,16 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_Pol
switch (mission_item_int.command) {
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
ret.type = AC_PolyFenceType::POLYGON_INCLUSION;
if (mission_item_int.param1 > 255) {
return MAV_MISSION_INVALID_PARAM1;
}
ret.vertex_count = mission_item_int.param1;
break;
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
ret.type = AC_PolyFenceType::POLYGON_EXCLUSION;
if (mission_item_int.param1 > 255) {
return MAV_MISSION_INVALID_PARAM1;
}
ret.vertex_count = mission_item_int.param1;
break;
case MAV_CMD_NAV_FENCE_RETURN_POINT:
@ -218,7 +225,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const u
return MAV_MISSION_ERROR;
}
const uint16_t allocation_size = count * sizeof(AC_PolyFenceItem);
const uint32_t allocation_size = count * sizeof(AC_PolyFenceItem);
if (allocation_size != 0) {
_new_items = (AC_PolyFenceItem*)malloc(allocation_size);
if (_new_items == nullptr) {

1
libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
View File

@ -143,6 +143,7 @@ bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq,
ret_packet.x = rallypoint.lat;
ret_packet.y = rallypoint.lng;
ret_packet.z = rallypoint.alt;
ret_packet.mission_type = MAV_MISSION_TYPE_RALLY;
return true;
}

1
libraries/GCS_MAVLink/ap_message.h
View File

@ -104,5 +104,6 @@ enum ap_message : uint8_t {
#endif
MSG_AIRSPEED,
MSG_AVAILABLE_MODES,
MSG_AVAILABLE_MODES_MONITOR,
MSG_LAST // MSG_LAST must be the last entry in this enum
};

1
libraries/RC_Channel/RC_Channel.h
View File

@ -260,6 +260,7 @@ public:
FLIGHTMODE_PAUSE = 178, // e.g. pause movement towards waypoint
ICE_START_STOP = 179, // AP_ICEngine start stop
AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains
QUICKTUNE = 181, //quicktune 3 position switch
// inputs from 200 will eventually used to replace RCMAP

4
libraries/SITL/SIM_Precland.cpp
View File

@ -85,7 +85,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
// @User: Advanced
AP_GROUPINFO("TYPE", 6, SIM_Precland, _type, SIM_Precland::PRECLAND_TYPE_CYLINDER),
// @Param: ALT_LIMIT
// @Param: ALT_LMT
// @DisplayName: Precland device alt range
// @Description: Precland device maximum range altitude
// @Units: m
@ -93,7 +93,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ALT_LMT", 7, SIM_Precland, _alt_limit, 15),
// @Param: DIST_LIMIT
// @Param: DIST_LMT
// @DisplayName: Precland device lateral range
// @Description: Precland device maximum lateral range
// @Units: m