diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index 26185663fe7ac..91926070e1227 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -175,6 +175,7 @@ jobs: - name: Install Prerequisites shell: bash run: | + sudo apt-get update sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10 update-alternatives --query python diff --git a/AntennaTracker/version.h b/AntennaTracker/version.h index 2a6f77e1655d6..eae1a8cad548f 100644 --- a/AntennaTracker/version.h +++ b/AntennaTracker/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "AntennaTracker V4.5.0-dev" +#define THISFIRMWARE "AntennaTracker V4.6.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 6 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1a4fefe7dc64f..c2cff5682e817 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -529,6 +529,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("FWD_THR_USE", 37, QuadPlane, q_fwd_thr_use, uint8_t(FwdThrUse::OFF)), + // @Param: BCK_PIT_LIM + // @DisplayName: Q mode rearward pitch limit + // @Description: This sets the maximum number of degrees of back or pitch up in Q modes when the airspeed is at AIRSPEED_MIN, and is used to prevent excessive sutructural loads when pitching up decelerate. If airspeed is above or below AIRSPEED_MIN, the pitch up/back will be adjusted according to the formula pitch_limit = Q_BCK_PIT_LIM * (AIRSPEED_MIN / IAS)^2. The backwards/up pitch limit controlled by this parameter is in addition to limiting applied by PTCH_LIM_MAX_DEG and Q_ANGLE_MAX. The BCK_PIT_LIM limit is only applied when Q_FWD_THR_USE is set to 1 or 2 and the vehicle is flying in a mode that uses forward throttle instead of forward tilt to generate forward speed. Set to a non positive value 0 to deactivate this limit. + // @Units: deg + // @Range: 0.0 15.0 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("BCK_PIT_LIM", 38, QuadPlane, q_bck_pitch_lim, 10.0f), + AP_GROUPEND }; @@ -3016,21 +3025,55 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); // Relax forward tilt limit if the position controller is saturating in the forward direction because - // the forward thrust motor could be failed - const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim; - if (is_positive(fwd_tilt_range_cd)) { - // rate limit the forward tilt change to slew between the motor good and motor failed - // value over 10 seconds - const bool fwd_limited = plane.quadplane.pos_control->is_active_xy() and plane.quadplane.pos_control->get_fwd_pitch_is_limited(); - const float fwd_pitch_lim_cd_tgt = fwd_limited ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; - const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; - q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); - // Don't let the forward pitch limit be more than the forward pitch demand before limiting to - // avoid opening up the limit more than necessary - q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim)); - } else { - // take the lesser of the two limits - q_fwd_pitch_lim_cd = (float)aparm.angle_max; + // the forward thrust motor could be failed. Do not do this with tilt rotors because they do not rely on + // forward throttle during VTOL flight + if (!tiltrotor.enabled()) { + const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim; + if (is_positive(fwd_tilt_range_cd)) { + // rate limit the forward tilt change to slew between the motor good and motor failed + // value over 10 seconds + const bool fwd_limited = plane.quadplane.pos_control->is_active_xy() and plane.quadplane.pos_control->get_fwd_pitch_is_limited(); + const float fwd_pitch_lim_cd_tgt = fwd_limited ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; + const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; + q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); + // Don't let the forward pitch limit be more than the forward pitch demand before limiting to + // avoid opening up the limit more than necessary + q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim)); + } else { + // take the lesser of the two limits + q_fwd_pitch_lim_cd = (float)aparm.angle_max; + } + } + + // Prevent the wing from being overloaded when braking from high speed in a VTOL mode + float nav_pitch_upper_limit_cd = 100.0f * q_bck_pitch_lim; + float aspeed; + if (is_positive(q_bck_pitch_lim) && ahrs.airspeed_estimate(aspeed)) { + const float reference_speed = MAX(plane.aparm.airspeed_min, MIN_AIRSPEED_MIN); + float speed_scaler = sq(reference_speed / MAX(aspeed, 0.1f)); + nav_pitch_upper_limit_cd *= speed_scaler; + nav_pitch_upper_limit_cd = MIN(nav_pitch_upper_limit_cd, (float)aparm.angle_max); + + const float tconst = 0.5f; + const float dt = AP_HAL::millis() - q_pitch_limit_update_ms; + q_pitch_limit_update_ms = AP_HAL::millis(); + if (is_positive(dt)) { + const float coef = dt / (dt + tconst); + q_bck_pitch_lim_cd = (1.0f - coef) * q_bck_pitch_lim_cd + coef * nav_pitch_upper_limit_cd; + } + + plane.nav_pitch_cd = MIN(plane.nav_pitch_cd, (int32_t)q_bck_pitch_lim_cd); + +#if HAL_LOGGING_ENABLED + AP::logger().WriteStreaming("QBRK", + "TimeUS,SpdScaler,NPULCD,QBPLCD,NPCD", // labels + "Qffii", // fmt + AP_HAL::micros64(), + (double)speed_scaler, + (double)nav_pitch_upper_limit_cd, + (int32_t)q_bck_pitch_lim_cd, + (int32_t)plane.nav_pitch_cd); +#endif } float fwd_thr_scaler; @@ -3054,14 +3097,15 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { #if HAL_LOGGING_ENABLED // Diagnostics logging - remove when feature is fully flight tested. AP::logger().WriteStreaming("FWDT", - "TimeUS,fts,qfplcd,npllcd,npcd,qft", // labels - "Qfffff", // fmt + "TimeUS,fts,qfplcd,npllcd,npcd,qft,npulcd", // labels + "Qffffff", // fmt AP_HAL::micros64(), (double)fwd_thr_scaler, (double)q_fwd_pitch_lim_cd, (double)nav_pitch_lower_limit_cd, (double)plane.nav_pitch_cd, - (double)q_fwd_throttle); + (double)q_fwd_throttle, + (float)nav_pitch_upper_limit_cd); #endif plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, (int32_t)nav_pitch_lower_limit_cd); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 63a0cb235a272..5b2b35fa92dff 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -403,6 +403,9 @@ class QuadPlane // limit applied to forward pitch to prevent wing producing negative lift AP_Float q_fwd_pitch_lim; + // limit applied to back pitch to prevent wing producing excessive lift + AP_Float q_bck_pitch_lim; + // which fwd throttle handling method is active enum class ActiveFwdThr : uint8_t { NONE = 0, @@ -441,6 +444,8 @@ class QuadPlane float q_fwd_throttle; // forward throttle used in q modes float q_fwd_pitch_lim_cd; // forward pitch limit applied when using q_fwd_throttle + float q_bck_pitch_lim_cd; // backward pitch limit applied when using Q_BCK_PIT_LIM + uint32_t q_pitch_limit_update_ms; // last time the backward pitch limit was updated // when did we last run the attitude controller? uint32_t last_att_control_ms; diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 252044e1efceb..5119d6ee8cc05 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,18 @@ +Release 1.7.0 26th February 2023 +-------------------------------- + +This is a major AP_Periph release with the following key changes: + +- fixed DroneCAN packet parsing bug when dealing with corrupt packets +- added BATT_HIDE_MASK parameter +- support IPv4 networking in AP_Periph and PPP gateway +- support per-cell battery monitoring +- rate limit EFI updates +- support serial tunnelling over DroneCAN +- support relays over DroneCAN via hardpoint messages +- support mapping MAVLink SEND_TEXT to DroneCAN debug levels +- fixed CANFD timings + Release 1.6.0 8th September 2023 -------------------------------- diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index ed2dccddb44c4..2eb0a235a8ce5 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -7,7 +7,7 @@ #include "ap_version.h" #include -#define THISFIRMWARE "AP_Periph V1.7.0-dev" +#define THISFIRMWARE "AP_Periph V1.8.0-dev" // defines needed due to lack of GCS includes #ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE @@ -17,10 +17,10 @@ #endif // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 1,7,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 1,8,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 1 -#define FW_MINOR 7 +#define FW_MINOR 8 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index b49010ae06001..67cb61428248b 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -52,7 +52,7 @@ def _vehicle_index(vehicle): return _vehicle_indexes[vehicle] # note that AP_NavEKF3_core.h is needed for AP_NavEKF3_feature.h -_vehicle_macros = ['SKETCHNAME', 'SKETCH', 'APM_BUILD_DIRECTORY', +_vehicle_macros = ['APM_BUILD_DIRECTORY', 'AP_BUILD_TARGET_NAME', 'APM_BUILD_TYPE', 'APM_BUILD_COPTER_OR_HELI', 'AP_NavEKF3_core.h', 'lua_generated_bindings.h'] _macros_re = re.compile(r'\b(%s)\b' % '|'.join(_vehicle_macros)) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 882e2f86ca408..490fc21d9e287 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -125,14 +125,12 @@ def get_legacy_defines(sketch_name, bld): if bld.cmd == 'heli' or 'heli' in bld.targets: return [ 'APM_BUILD_DIRECTORY=APM_BUILD_Heli', - 'SKETCH="' + sketch_name + '"', - 'SKETCHNAME="' + sketch_name + '"', + 'AP_BUILD_TARGET_NAME="' + sketch_name + '"', ] return [ 'APM_BUILD_DIRECTORY=APM_BUILD_' + sketch_name, - 'SKETCH="' + sketch_name + '"', - 'SKETCHNAME="' + sketch_name + '"', + 'AP_BUILD_TARGET_NAME="' + sketch_name + '"', ] IGNORED_AP_LIBRARIES = [ diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index dbffada9201c8..e886a7f1682ee 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7966,7 +7966,7 @@ def statustext_in_collections(self, text, regex=False): if "STATUSTEXT" not in c.collections: raise NotAchievedException("Asked to check context but it isn't collecting!") for x in c.collections["STATUSTEXT"]: - self.progress(" statustext=%s vs text=%s" % (x.text, text)) + self.progress(" statustext want=(%s) got=(%s)" % (x.text, text)) if regex: if re.match(text, x.text): return x diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index c9966cbefd91a..55359a1edb6fb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -19,8 +19,8 @@ #ifndef AC_ATC_MULTI_RATE_RP_IMAX # define AC_ATC_MULTI_RATE_RP_IMAX 0.5f #endif -#ifndef AC_ATC_MULTI_RATE_RP_FILT_HZ - # define AC_ATC_MULTI_RATE_RP_FILT_HZ 20.0f +#ifndef AC_ATC_MULTI_RATE_RPY_FILT_HZ + # define AC_ATC_MULTI_RATE_RPY_FILT_HZ 20.0f #endif #ifndef AC_ATC_MULTI_RATE_YAW_P # define AC_ATC_MULTI_RATE_YAW_P 0.180f @@ -106,9 +106,9 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { .d = AC_ATC_MULTI_RATE_RP_D, .ff = 0.0f, .imax = AC_ATC_MULTI_RATE_RP_IMAX, - .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .filt_E_hz = 0.0f, - .filt_D_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .srmax = 0, .srtau = 1.0 } @@ -120,9 +120,9 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { .d = AC_ATC_MULTI_RATE_RP_D, .ff = 0.0f, .imax = AC_ATC_MULTI_RATE_RP_IMAX, - .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .filt_E_hz = 0.0f, - .filt_D_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .srmax = 0, .srtau = 1.0 } @@ -135,9 +135,9 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { .d = AC_ATC_MULTI_RATE_YAW_D, .ff = 0.0f, .imax = AC_ATC_MULTI_RATE_YAW_IMAX, - .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .filt_E_hz = AC_ATC_MULTI_RATE_YAW_FILT_HZ, - .filt_D_hz = 0.0f, + .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .srmax = 0, .srtau = 1.0 } diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp index cd6d00cd2af04..10765e8ed0404 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -323,9 +323,9 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_V _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), - _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f), - _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f), - _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f) + _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f), + _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f), + _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f) { _dt = dt; AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_CustomControl/README.md b/libraries/AC_CustomControl/README.md index f1cf85fa10ce4..2d2d4a892d135 100644 --- a/libraries/AC_CustomControl/README.md +++ b/libraries/AC_CustomControl/README.md @@ -243,9 +243,9 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), - _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt), - _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt), - _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt) + _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt), + _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt), + _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp index 7b7b088e48d24..ad193a4aa33aa 100644 --- a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp +++ b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp @@ -70,11 +70,11 @@ void setup() void loop() { // setup (unfortunately must be done here as we cannot create a global AC_PID object) - AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER); - AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER); + AC_PID *pid = new AC_PID(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER); + // AC_HELI_PID *heli_pid= new AC_HELI_PID(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER); // display PID gains - hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax()); + hal.console->printf("P %f I %f D %f imax %f\n", (double)pid->kP(), (double)pid->kI(), (double)pid->kD(), (double)pid->imax()); RC_Channel *c = rc().channel(0); if (c == nullptr) { @@ -91,10 +91,10 @@ void loop() rc().read_input(); // poll the radio for new values const uint16_t radio_in = c->get_radio_in(); const int16_t error = radio_in - radio_trim; - pid.update_error(error, TEST_DT); - const float control_P = pid.get_p(); - const float control_I = pid.get_i(); - const float control_D = pid.get_d(); + pid->update_error(error, TEST_DT); + const float control_P = pid->get_p(); + const float control_I = pid->get_i(); + const float control_D = pid->get_d(); // display pid results hal.console->printf("radio: %d\t err: %d\t pid:%4.2f (p:%4.2f i:%4.2f d:%4.2f)\n", diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 4af6e8bd72dcb..a82a014dedd64 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -214,24 +214,26 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const // get an individual ESC's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { + || telemdata.stale() + || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { return false; } - temp = _telem_data[esc_index].temperature_cdeg; + temp = telemdata.temperature_cdeg; return true; } // get an individual motor's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { + || telemdata.stale() + || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { return false; } - temp = _telem_data[esc_index].motor_temp_cdeg; + temp = telemdata.motor_temp_cdeg; return true; } @@ -254,48 +256,52 @@ bool AP_ESC_Telem::get_highest_motor_temperature(int16_t& temp) const // get an individual ESC's current in Ampere if available, returns true on success bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { return false; } - amps = _telem_data[esc_index].current; + amps = telemdata.current; return true; } // get an individual ESC's voltage in Volt if available, returns true on success bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { return false; } - volts = _telem_data[esc_index].voltage; + volts = telemdata.voltage; return true; } // get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { return false; } - consumption_mah = _telem_data[esc_index].consumption_mah; + consumption_mah = telemdata.consumption_mah; return true; } // get an individual ESC's usage time in seconds if available, returns true on success bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { return false; } - usage_s = _telem_data[esc_index].usage_s; + usage_s = telemdata.usage_s; return true; } @@ -312,8 +318,8 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) const uint32_t now_us = AP_HAL::micros(); // loop through groups of 4 ESCs - const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1); - const uint8_t num_idx = ESC_TELEM_MAX_ESCS/4; + const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS - 1); + const uint8_t num_idx = ESC_TELEM_MAX_ESCS / 4; for (uint8_t idx = 0; idx < num_idx; idx++) { const uint8_t i = (next_idx + idx) % num_idx; @@ -349,16 +355,17 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) if (esc_id >= ESC_TELEM_MAX_ESCS) { continue; } + volatile AP_ESC_Telem_Backend::TelemetryData const &telemdata = _telem_data[esc_id]; - s.temperature[j] = _telem_data[esc_id].temperature_cdeg / 100; - s.voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX); - s.current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX); - s.totalcurrent[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX); - float rpmf = 0.0f; + s.temperature[j] = telemdata.temperature_cdeg / 100; + s.voltage[j] = constrain_float(telemdata.voltage * 100.0f, 0, UINT16_MAX); + s.current[j] = constrain_float(telemdata.current * 100.0f, 0, UINT16_MAX); + s.totalcurrent[j] = constrain_float(telemdata.consumption_mah, 0, UINT16_MAX); + float rpmf; if (get_rpm(esc_id, rpmf)) { s.rpm[j] = constrain_float(rpmf, 0, UINT16_MAX); } - s.count[j] = _telem_data[esc_id].count; + s.count[j] = telemdata.count; } // make sure a msg hasn't been extended @@ -425,41 +432,42 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem } _have_data = true; + volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[esc_index]; #if AP_TEMPERATURE_SENSOR_ENABLED // always allow external data. Block "internal" if external has ever its ever been set externally then ignore normal "internal" updates const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) || - ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) || - ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); #else const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE); #endif if (has_temperature) { - _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; + telemdata.temperature_cdeg = new_data.temperature_cdeg; } if (has_motor_temperature) { - _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; + telemdata.motor_temp_cdeg = new_data.motor_temp_cdeg; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) { - _telem_data[esc_index].voltage = new_data.voltage; + telemdata.voltage = new_data.voltage; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CURRENT) { - _telem_data[esc_index].current = new_data.current; + telemdata.current = new_data.current; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION) { - _telem_data[esc_index].consumption_mah = new_data.consumption_mah; + telemdata.consumption_mah = new_data.consumption_mah; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { - _telem_data[esc_index].usage_s = new_data.usage_s; + telemdata.usage_s = new_data.usage_s; } - _telem_data[esc_index].count++; - _telem_data[esc_index].types |= data_mask; - _telem_data[esc_index].last_update_ms = AP_HAL::millis(); + telemdata.count++; + telemdata.types |= data_mask; + telemdata.last_update_ms = AP_HAL::millis(); } // record an update to the RPM together with timestamp, this allows the notch values to be slewed @@ -492,20 +500,19 @@ void AP_ESC_Telem::update() { #if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); -#endif - - const uint32_t now_us = AP_HAL::micros(); + const uint64_t now_us64 = AP_HAL::micros64(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { -#if HAL_LOGGING_ENABLED + const volatile AP_ESC_Telem_Backend::RpmData &rpmdata = _rpm_data[i]; + const volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i]; // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { - if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] - || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { + if (telemdata.last_update_ms != _last_telem_log_ms[i] + || rpmdata.last_update_us != _last_rpm_log_us[i]) { - float rpm = 0.0f; + float rpm = AP::logger().quiet_nanf(); get_rpm(i, rpm); - float raw_rpm = 0.0f; + float raw_rpm = AP::logger().quiet_nanf(); get_raw_rpm(i, raw_rpm); // Write ESC status messages @@ -519,31 +526,35 @@ void AP_ESC_Telem::update() // error_rate is in percentage const struct log_Esc pkt{ LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)), - time_us : AP_HAL::micros64(), + time_us : now_us64, instance : i, rpm : rpm, raw_rpm : raw_rpm, - voltage : _telem_data[i].voltage, - current : _telem_data[i].current, - esc_temp : _telem_data[i].temperature_cdeg, - current_tot : _telem_data[i].consumption_mah, - motor_temp : _telem_data[i].motor_temp_cdeg, - error_rate : _rpm_data[i].error_rate + voltage : telemdata.voltage, + current : telemdata.current, + esc_temp : telemdata.temperature_cdeg, + current_tot : telemdata.consumption_mah, + motor_temp : telemdata.motor_temp_cdeg, + error_rate : rpmdata.error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); - _last_telem_log_ms[i] = _telem_data[i].last_update_ms; - _last_rpm_log_us[i] = _rpm_data[i].last_update_us; + _last_telem_log_ms[i] = telemdata.last_update_ms; + _last_rpm_log_us[i] = rpmdata.last_update_us; } } + } #endif // HAL_LOGGING_ENABLED + const uint32_t now_us = AP_HAL::micros(); + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + // Invalidate RPM data if not received for too long if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { _rpm_data[i].data_valid = false; } } } -bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) +bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) { // easy case, has the time window been crossed so it's invalid if ((now_us - instance.last_update_us) > timeout_us) { @@ -557,7 +568,7 @@ bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend: return instance.data_valid; } -bool AP_ESC_Telem::was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance) +bool AP_ESC_Telem::was_rpm_data_ever_reported(const volatile AP_ESC_Telem_Backend::RpmData &instance) { return instance.last_update_us > 0; } diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 94e98f0ac1d83..9b15335435200 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1633,7 +1633,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_ // we have one or more partial fragments already received // which conflict with the new fragment, discard previous fragments rtcm_buffer->fragment_count = 0; - rtcm_stats.fragments_discarded += rtcm_buffer->fragments_received; + rtcm_stats.fragments_discarded += __builtin_popcount(rtcm_buffer->fragments_received); rtcm_buffer->fragments_received = 0; } @@ -1662,7 +1662,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_ if (rtcm_buffer->fragment_count != 0 && rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) { // we have them all, inject - rtcm_stats.fragments_used += rtcm_buffer->fragments_received; + rtcm_stats.fragments_used += __builtin_popcount(rtcm_buffer->fragments_received); inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length); rtcm_buffer->fragment_count = 0; rtcm_buffer->fragments_received = 0; diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index eb741e1ae7c52..ea55e7140f08a 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -286,7 +286,7 @@ static void main_loop() hal.scheduler->set_system_initialized(); thread_running = true; - chRegSetThreadName(SKETCHNAME); + chRegSetThreadName(AP_BUILD_TARGET_NAME); /* switch to high priority for main loop diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index e4ee42cc4ddb4..7f8fc2819b9c8 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -31,9 +31,9 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; #ifndef HAL_STORAGE_FILE -// using SKETCHNAME allows the one microSD to be used +// using AP_BUILD_TARGET_NAME allows the one microSD to be used // for multiple vehicle types -#define HAL_STORAGE_FILE "/APM/" SKETCHNAME ".stg" +#define HAL_STORAGE_FILE "/APM/" AP_BUILD_TARGET_NAME ".stg" #endif #ifndef HAL_STORAGE_BACKUP_FOLDER diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 7c2cce5d2bfb3..98e3914332274 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -173,15 +173,20 @@ void Scheduler::delay(uint16_t ms) return; } - uint64_t start = AP_HAL::millis64(); + if (ms == 0) { + return; + } - while ((AP_HAL::millis64() - start) < ms) { + uint64_t now = AP_HAL::micros64(); + uint64_t end = now + 1000UL * ms + 1U; + do { // this yields the CPU to other apps - microsleep(1000); + microsleep(MIN(1000UL, end-now)); if (in_main_thread() && _min_delay_cb_ms <= ms) { call_delay_cb(); } - } + now = AP_HAL::micros64(); + } while (now < end); } void Scheduler::delay_microseconds(uint16_t us) diff --git a/libraries/AP_HAL_Linux/Storage.cpp b/libraries/AP_HAL_Linux/Storage.cpp index 4f9fc133a274a..4fc401c76405b 100644 --- a/libraries/AP_HAL_Linux/Storage.cpp +++ b/libraries/AP_HAL_Linux/Storage.cpp @@ -20,7 +20,7 @@ using namespace Linux; // name the storage file after the sketch so you can use the same board // card for ArduCopter and ArduPlane -#define STORAGE_FILE SKETCHNAME ".stg" +#define STORAGE_FILE AP_BUILD_TARGET_NAME ".stg" extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 96ec46d879d99..f4c5529197ff4 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -212,7 +212,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) // default to CMAC const char *home_str = nullptr; const char *model_str = nullptr; - const char *vehicle_str = SKETCH; + const char *vehicle_str = AP_BUILD_TARGET_NAME; _use_fg_view = false; char *autotest_dir = nullptr; _fg_address = "127.0.0.1"; diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 8845c9ba00fc2..54dd754e6200d 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1195,6 +1195,13 @@ struct PACKED log_VER { // @Field: Free: free stack // @Field: Name: thread name +// @LoggerMessage: FILE +// @Description: File data +// @Field: FileName: File name +// @Field: Offset: Offset into the file of this block +// @Field: Length: Length of this data block +// @Field: Data: File data of this block + // @LoggerMessage: SCR // @Description: Scripting runtime stats // @Field: TimeUS: Time since system startup @@ -1203,6 +1210,20 @@ struct PACKED log_VER { // @Field: Total_mem: total memory usage of all scripts // @Field: Run_mem: run memory usage +// @LoggerMessage: VER +// @Description: Ardupilot version +// @Field: TimeUS: Time since system startup +// @Field: BT: Board type +// @Field: BST: Board subtype +// @Field: Maj: Major version number +// @Field: Min: Minor version number +// @Field: Pat: Patch number +// @Field: FWT: Firmware type +// @Field: GH: Github commit +// @Field: FWS: Firmware version string +// @Field: APJ: Board ID +// @Field: BU: Build vehicle type + // @LoggerMessage: MOTB // @Description: Motor mixer information // @Field: TimeUS: Time since system startup diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index e107dffe5f4d6..9a2d09ba18847 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -201,6 +201,7 @@ void AP_MSP_Telem_Backend::update_home_pos(home_state_t &home_state) home_state.home_is_set = _ahrs.home_is_set(); } +#if AP_GPS_ENABLED void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state) { AP_GPS& gps = AP::gps(); @@ -220,6 +221,7 @@ void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state) gps_state.ground_course_cd = gps.ground_course_cd(); } } +#endif #if AP_BATTERY_ENABLED void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state) @@ -645,8 +647,10 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst) return MSP_RESULT_ERROR; } #endif - gps_state_t gps_state; + gps_state_t gps_state {}; +#if AP_GPS_ENABLED update_gps_state(gps_state); +#endif // handle airspeed override bool airspeed_en = false; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index c8df1a5c0742f..9708b60f164ef 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -268,11 +268,12 @@ void NavEKF3_core::FuseRngBcn() // Update the fusion report if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) { - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].testRatio = rngBcn.testRatio; + auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID]; + report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED; + report.innov = rngBcn.innov; + report.innovVar = rngBcn.varInnov; + report.rng = rngBcn.dataDelayed.rng; + report.testRatio = rngBcn.testRatio; } } } @@ -506,11 +507,12 @@ void NavEKF3_core::FuseRngBcnStatic() } // Update the fusion report if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) { - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].testRatio = rngBcn.testRatio; + auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID]; + report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED; + report.innov = rngBcn.innov; + report.innovVar = rngBcn.varInnov; + report.rng = rngBcn.dataDelayed.rng; + report.testRatio = rngBcn.testRatio; } } } diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index bbaec88f3e4d0..428f589b89270 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -403,82 +403,6 @@ void AP_PiccoloCAN::update() #endif // HAL_LOGGING_ENABLED } -#if HAL_GCS_ENABLED -// send ESC telemetry messages over MAVLink -void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) -{ - // Arrays to store ESC telemetry data - uint8_t temperature[4] {}; - uint16_t voltage[4] {}; - uint16_t rpm[4] {}; - uint16_t count[4] {}; - uint16_t current[4] {}; - uint16_t totalcurrent[4] {}; - - bool dataAvailable = false; - - uint8_t idx = 0; - - WITH_SEMAPHORE(_telem_sem); - - for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) { - - // Calculate index within storage array - idx = (ii % 4); - - AP_PiccoloCAN_ESC &esc = _escs[idx]; - - // Has the ESC been heard from recently? - if (is_esc_present(ii)) { - dataAvailable = true; - - // Provide the maximum ESC temperature in the telemetry stream - temperature[idx] = esc.temperature(); // Convert to C - voltage[idx] = esc.voltage() * 10; // Convert to cV - current[idx] = esc.current() * 10; // Convert to cA - totalcurrent[idx] = 0; - rpm[idx] = esc.rpm(); - count[idx] = 0; - } else { - temperature[idx] = 0; - voltage[idx] = 0; - current[idx] = 0; - totalcurrent[idx] = 0; - rpm[idx] = 0; - count[idx] = 0; - } - - // Send ESC telemetry in groups of 4 - if ((ii % 4) == 3) { - - if (dataAvailable) { - if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) { - continue; - } - - switch (ii) { - case 3: - mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 7: - mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 11: - mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 15: - mavlink_msg_esc_telemetry_13_to_16_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - default: - break; - } - } - - dataAvailable = false; - } - } -} -#endif // send servo messages over CAN void AP_PiccoloCAN::send_servo_messages(void) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 770f05dcf53bf..3615f3db4fa29 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -56,9 +56,6 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend // called from SRV_Channels void update(); - // send ESC telemetry messages over MAVLink - void send_esc_telemetry_mavlink(uint8_t mav_chan); - // return true if a particular servo is 'active' on the Piccolo interface bool is_servo_channel_active(uint8_t chan); @@ -125,14 +122,14 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend } _ecu_info; // Piccolo CAN parameters - AP_Int32 _esc_bm; //! ESC selection bitmask - AP_Int16 _esc_hz; //! ESC update rate (Hz) + AP_Int32 _esc_bm; //!< ESC selection bitmask + AP_Int16 _esc_hz; //!< ESC update rate (Hz) - AP_Int32 _srv_bm; //! Servo selection bitmask - AP_Int16 _srv_hz; //! Servo update rate (Hz) + AP_Int32 _srv_bm; //!< Servo selection bitmask + AP_Int16 _srv_hz; //!< Servo update rate (Hz) - AP_Int16 _ecu_id; //! ECU Node ID - AP_Int16 _ecu_hz; //! ECU update rate (Hz) + AP_Int16 _ecu_id; //!< ECU Node ID + AP_Int16 _ecu_hz; //!< ECU update rate (Hz) HAL_Semaphore _telem_sem; }; diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h index e0b3cd9f169bf..fff6e17539568 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h @@ -27,18 +27,18 @@ #if HAL_PICCOLO_CAN_ENABLE -// Piccolo message groups form part of the CAN ID of each frame +//! Piccolo message groups form part of the CAN ID of each frame enum class PiccoloCAN_MessageGroup : uint8_t { - SIMULATOR = 0x00, // Simulator messages - SENSOR = 0x04, // External sensors - ACTUATOR = 0x07, // Actuators (e.g. ESC / servo) - ECU_OUT = 0x08, // Messages *from* an ECU - ECU_IN = 0x09, // Message *to* an ECU + SIMULATOR = 0x00, //!< Simulator messages + SENSOR = 0x04, //!< External sensors + ACTUATOR = 0x07, //!< Actuators (e.g. ESC / servo) + ECU_OUT = 0x08, //!< Messages *from* an ECU + ECU_IN = 0x09, //!< Message *to* an ECU - SYSTEM = 0x19, // System messages (e.g. bootloader) + SYSTEM = 0x19, //!< System messages (e.g. bootloader) }; -// Piccolo actuator types differentiate between actuator frames +//! Piccolo actuator types differentiate between actuator frames enum class PiccoloCAN_ActuatorType : uint8_t { SERVO = 0x00, ESC = 0x20, diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp index 8f45dda7002be..24c4ed760ca7e 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp @@ -20,7 +20,7 @@ #if HAL_PICCOLO_CAN_ENABLE /* - * Decode a recevied CAN frame. + * Decode a received CAN frame. * It is assumed at this point that the received frame is intended for *this* ESC */ bool AP_PiccoloCAN_ESC::handle_can_frame(AP_HAL::CANFrame &frame) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 81f446d53aee0..7e53bda5e7979 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -425,10 +425,10 @@ bool AP_RCProtocol::new_input() #endif }; for (const auto protocol : pollable) { - _new_input = detect_async_protocol(protocol); - if (!_new_input) { + if (!detect_async_protocol(protocol)) { continue; } + _new_input = true; _last_input_ms = AP_HAL::millis(); break; } diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index dd9c8b428e68d..82f1aa06cae23 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -246,7 +246,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: OPTIONS // @DisplayName: Extra TECS options // @Description: This allows the enabling of special features in the speed/height controller. - // @Bitmask: 0:GliderOnly + // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup // @User: Advanced AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0), @@ -458,6 +458,11 @@ void AP_TECS::_update_speed(float DT) void AP_TECS::_update_speed_demand(void) { + if (_options & OPTION_DESCENT_SPEEDUP) { + // Allow demanded speed to go to maximum when descending at maximum descent rate + _TAS_dem = _TAS_dem + (_TASmax - _TAS_dem) * _sink_fraction; + } + // Set the airspeed demand to the minimum value if an underspeed condition exists // or a bad descent condition exists // This will minimise the rate of descent resulting from an engine failure, @@ -532,9 +537,18 @@ void AP_TECS::_update_height_demand(void) // Limit height rate of change if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) { _hgt_dem_rate_ltd = _hgt_dem_rate_ltd + _climb_rate_limit * _DT; + _sink_fraction = 0.0f; } else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) { _hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT; + _sink_fraction = 1.0f; } else { + const float numerator = hgt_dem - _hgt_dem_rate_ltd; + const float denominator = - _sink_rate_limit * _DT; + if (is_negative(numerator) && is_negative(denominator)) { + _sink_fraction = numerator / denominator; + } else { + _sink_fraction = 0.0f; + } _hgt_dem_rate_ltd = hgt_dem; } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index b392a0bfc555b..4470b700f546f 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -197,7 +197,8 @@ class AP_TECS { AP_Float _hgt_dem_tconst; enum { - OPTION_GLIDER_ONLY=(1<<0) + OPTION_GLIDER_ONLY=(1<<0), + OPTION_DESCENT_SPEEDUP=(1<<1) }; AP_Float _pitch_ff_v0; @@ -334,6 +335,7 @@ class AP_TECS { // true when a reset of airspeed and height states to current is performed on this frame bool reset:1; + }; union { struct flags _flags; @@ -389,6 +391,7 @@ class AP_TECS { // used to scale max climb and sink limits to match vehicle ability float _max_climb_scaler; float _max_sink_scaler; + float _sink_fraction; // Specific energy error quantities float _STE_error;