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

Merge branch 'master' into master

This commit is contained in:
Józef Wołoch 2024-12-17 18:15:56 +01:00 committed by GitHub
parent fd254fbbb0 7c084e9955
commit b92f06ac68
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
221 changed files with 6165 additions and 678 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -187,7 +187,6 @@ jobs:
PATH: /usr/bin:$(cygpath ${SYSTEMROOT})/system32
shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}'
run: >-
ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip &&
python3 -m pip install --progress-bar off empy==3.3.4 pexpect &&
python3 -m pip install --progress-bar off dronecan --upgrade &&
cp /usr/bin/ccache /usr/local/bin/ &&

18
.github/workflows/esp32_build.yml vendored
View File

@ -175,9 +175,7 @@ jobs:
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
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 cmake
python3 --version
pip3 install gevent
@ -186,24 +184,10 @@ jobs:
sudo apt-get update
sudo apt-get install python3.11 python3.11-venv python3.11-distutils -y
sudo apt-get install python3 python3-pip python3-venv python3-setuptools python3-serial python3-cryptography python3-future python3-pyparsing python3-pyelftools
update-alternatives --query python
pip3 install gevent
python3 --version
python3.11 --version
which python3.11
sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.11 11
update-alternatives --query python
rm -rf /usr/local/bin/cmake
sudo apt-get remove --purge --auto-remove cmake
sudo apt-get update && \
sudo apt-get install -y software-properties-common lsb-release && \
sudo apt-get clean all
wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null
sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main"
sudo apt-get update
sudo apt-get install cmake
git submodule update --init --recursive --depth=1
./Tools/scripts/esp32_get_idf.sh

4
.gitignore vendored
View File

@ -171,7 +171,3 @@ ENV/
env.bak/
venv.bak/
autotest_result_*_junit.xml
# Ignore ESP-IDF SDK defines
/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig
/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig

49
AntennaTracker/ReleaseNotes.txt
View File

@ -1,6 +1,53 @@
Antenna Tracker Release Notes:
------------------------------------------------------------------
Release 4.6.0-beta 13 Nov 2024
Release 4.6.0-beta2 11 Dec 2024
Changes from 4.6.0-beta1
1) Board specfic changes
- FoxeerF405v2 supports BMP280 baro
- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro
- MUPilot support
- SkySakura H743 support
- TBS Lucid H7 support
- VUAV-V7pro README documentation fixed
- X-MAV AP-H743v2 CAN pin definition fixed
2) Copter specific enhancements and bug fixes
- AutoTune fix for calc of maximum angular acceleration
- Advanced Failsafe customer build server option
3) Plane related enhancements and bug fixes
- QuadPlane fix for QLand getting stuck in pilot repositioning
- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards)
- Takeoff direction fixed when no yaw source
- TECS correctly handles home altitude changes
4) Bug Fixes and minor enhancements
- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor
- CAN frame logging added to ease support
- CRSF reconnection after failsafe fixed
- EKF3 position and velocity resets default to user defined source
- Ethernet IP address default 192.168.144.x
- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed)
- Fence pre-arm check that vehicle is within polygon fence
- Fence handling of more than 256 items fixed
- FFT protection against divide-by-zero in Jain estimator
- Frsky telemetry apparent wind speed fixed
- Inertial sensors stop sensor converging if motors arm
- Inertial sensors check for changes to notch filters fixed
- Real Time Clock allowed to shift forward when disarmed
- ROS2/DDS get/set parameter service added
- Scripting gets memory handling improvements
- Scripting promote video-stream-information to applet
- Topotek gimbal driver uses GIA message to retrieve current angle
- Tramp VTX OSD power indicator fixed
------------------------------------------------------------------
Release 4.6.0-beta1 13 Nov 2024
Changes from 4.5.7

6
ArduCopter/Parameters.cpp
View File

@ -161,7 +161,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FS_GCS_ENABLE
// @DisplayName: Ground Station Failsafe Enable
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled.
// @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START or RTL,7:Brake or Land
// @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL,7:Brake or Land
// @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
@ -225,7 +225,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FS_THR_ENABLE
// @DisplayName: Throttle Failsafe Enable
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
// @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START or RTL,7:Enabled always Brake or Land
// @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START/DO_RETURN_PATH_START or RTL,7:Enabled always Brake or Land
// @User: Standard
GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL),
@ -1044,7 +1044,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: FS_DR_ENABLE
// @DisplayName: DeadReckon Failsafe Action
// @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates
// @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL
// @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL
// @User: Standard
AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL),

49
ArduCopter/ReleaseNotes.txt
View File

@ -1,6 +1,53 @@
ArduPilot Copter Release Notes:
------------------------------------------------------------------
Release 4.6.0-beta 13 Nov 2024
Release 4.6.0-beta2 11 Dec 2024
Changes from 4.6.0-beta1
1) Board specfic changes
- FoxeerF405v2 supports BMP280 baro
- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro
- MUPilot support
- SkySakura H743 support
- TBS Lucid H7 support
- VUAV-V7pro README documentation fixed
- X-MAV AP-H743v2 CAN pin definition fixed
2) Copter specific enhancements and bug fixes
- AutoTune fix for calc of maximum angular acceleration
- Advanced Failsafe customer build server option
3) Plane related enhancements and bug fixes
- QuadPlane fix for QLand getting stuck in pilot repositioning
- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards)
- Takeoff direction fixed when no yaw source
- TECS correctly handles home altitude changes
4) Bug Fixes and minor enhancements
- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor
- CAN frame logging added to ease support
- CRSF reconnection after failsafe fixed
- EKF3 position and velocity resets default to user defined source
- Ethernet IP address default 192.168.144.x
- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed)
- Fence pre-arm check that vehicle is within polygon fence
- Fence handling of more than 256 items fixed
- FFT protection against divide-by-zero in Jain estimator
- Frsky telemetry apparent wind speed fixed
- Inertial sensors stop sensor converging if motors arm
- Inertial sensors check for changes to notch filters fixed
- Real Time Clock allowed to shift forward when disarmed
- ROS2/DDS get/set parameter service added
- Scripting gets memory handling improvements
- Scripting promote video-stream-information to applet
- Topotek gimbal driver uses GIA message to retrieve current angle
- Tramp VTX OSD power indicator fixed
------------------------------------------------------------------
Release 4.6.0-beta1 13 Nov 2024
Changes from 4.5.7

9
ArduPlane/ArduPlane.cpp
View File

@ -789,7 +789,16 @@ float Plane::tecs_hgt_afe(void)
coming.
*/
float hgt_afe;
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
// if external HAGL is active use that
if (get_external_HAGL(hgt_afe)) {
return hgt_afe;
}
#endif
hgt_afe = height_above_target();
#if AP_RANGEFINDER_ENABLED
hgt_afe -= rangefinder_correction();

2
ArduPlane/Parameters.cpp
View File

@ -745,7 +745,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: RTL_AUTOLAND
// @DisplayName: RTL auto land
// @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation).
// @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation).If this is set to 0 and there is a DO_LAND_START or DO_RETURN_PATH_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around (see wiki for aborting autolandings) without it changing RTL behaviour.
// @Values: 0:Disable,1:Fly HOME then land via DO_LAND_START mission item, 2:Go directly to landing sequence via DO_LAND_START mission item, 3:OnlyForGoAround, 4:Go directly to landing sequence via DO_RETURN_PATH_START mission item
// @User: Standard
GSCALAR(rtl_autoland, "RTL_AUTOLAND", float(RtlAutoland::RTL_DISABLE)),

49
ArduPlane/ReleaseNotes.txt
View File

@ -1,6 +1,53 @@
ArduPilot Plane Release Notes:
------------------------------------------------------------------
Release 4.6.0-beta 13 Nov 2024
Release 4.6.0-beta2 11 Dec 2024
Changes from 4.6.0-beta1
1) Board specfic changes
- FoxeerF405v2 supports BMP280 baro
- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro
- MUPilot support
- SkySakura H743 support
- TBS Lucid H7 support
- VUAV-V7pro README documentation fixed
- X-MAV AP-H743v2 CAN pin definition fixed
2) Copter specific enhancements and bug fixes
- AutoTune fix for calc of maximum angular acceleration
- Advanced Failsafe customer build server option
3) Plane related enhancements and bug fixes
- QuadPlane fix for QLand getting stuck in pilot repositioning
- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards)
- Takeoff direction fixed when no yaw source
- TECS correctly handles home altitude changes
4) Bug Fixes and minor enhancements
- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor
- CAN frame logging added to ease support
- CRSF reconnection after failsafe fixed
- EKF3 position and velocity resets default to user defined source
- Ethernet IP address default 192.168.144.x
- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed)
- Fence pre-arm check that vehicle is within polygon fence
- Fence handling of more than 256 items fixed
- FFT protection against divide-by-zero in Jain estimator
- Frsky telemetry apparent wind speed fixed
- Inertial sensors stop sensor converging if motors arm
- Inertial sensors check for changes to notch filters fixed
- Real Time Clock allowed to shift forward when disarmed
- ROS2/DDS get/set parameter service added
- Scripting gets memory handling improvements
- Scripting promote video-stream-information to applet
- Topotek gimbal driver uses GIA message to retrieve current angle
- Tramp VTX OSD power indicator fixed
------------------------------------------------------------------
Release 4.6.0-beta1 13 Nov 2024
Changes from 4.5.7

17
ArduPlane/mode_takeoff.cpp
View File

@ -83,9 +83,12 @@ void ModeTakeoff::update()
if (!takeoff_mode_setup) {
plane.auto_state.takeoff_altitude_rel_cm = alt * 100;
const uint16_t altitude = plane.relative_ground_altitude(false,true);
const float direction = degrees(ahrs.get_yaw());
const Vector2f &groundspeed2d = ahrs.groundspeed_vector();
const float direction = wrap_360(degrees(groundspeed2d.angle()));
const float groundspeed = groundspeed2d.length();
// see if we will skip takeoff as already flying
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && groundspeed > 3) {
if (altitude >= alt) {
gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering");
plane.next_WP_loc = plane.current_loc;
@ -115,7 +118,15 @@ void ModeTakeoff::update()
plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
if (!plane.throttle_suppressed) {
/*
don't lock in the takeoff loiter location until we reach
a ground speed of AIRSPEED_MIN*0.3 to cope with aircraft
with no compass or poor compass. If flying in a very
strong headwind then the is_flying() check above will
eventually trigger
*/
if (!plane.throttle_suppressed &&
groundspeed > plane.aparm.airspeed_min*0.3) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
alt, dist, direction);
plane.takeoff_state.start_time_ms = millis();

67
Rover/GCS_Mavlink.cpp
View File

@ -185,6 +185,65 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
distance,
voltage);
}
void GCS_MAVLINK_Rover::send_water_depth() const
{
if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
return;
}
// only send for boats:
if (!rover.is_boat()) {
return;
}
RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder == nullptr) {
return;
}
// depth can only be measured by a downward-facing rangefinder:
if (!rangefinder->has_orientation(ROTATION_PITCH_270)) {
return;
}
// get position
const AP_AHRS &ahrs = AP::ahrs();
Location loc;
IGNORE_RETURN(ahrs.get_location(loc));
for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);
if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
continue;
}
// get temperature
float temp_C;
if (!s->get_temp(temp_C)) {
temp_C = 0.0f;
}
const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
mavlink_msg_water_depth_send(
chan,
AP_HAL::millis(), // time since system boot TODO: take time of measurement
i, // rangefinder instance
sensor_healthy, // sensor healthy
loc.lat, // latitude of vehicle
loc.lng, // longitude of vehicle
loc.alt * 0.01f, // altitude of vehicle (MSL)
ahrs.get_roll(), // roll in radians
ahrs.get_pitch(), // pitch in radians
ahrs.get_yaw(), // yaw in radians
s->distance(), // distance in meters
temp_C); // temperature in degC
}
}
#endif // AP_RANGEFINDER_ENABLED
/*
@ -400,6 +459,13 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
}
#endif
#if AP_RANGEFINDER_ENABLED
case MSG_WATER_DEPTH:
CHECK_PAYLOAD_SIZE(WATER_DEPTH);
send_water_depth();
break;
#endif // AP_RANGEFINDER_ENABLED
default:
return GCS_MAVLINK::try_send_message(id);
}
@ -589,6 +655,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
MSG_WIND,
#if AP_RANGEFINDER_ENABLED
MSG_RANGEFINDER,
MSG_WATER_DEPTH,
#endif
MSG_DISTANCE_SENSOR,
MSG_SYSTEM_TIME,

3
Rover/GCS_Mavlink.h
View File

@ -44,6 +44,9 @@ protected:
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
// send WATER_DEPTH - metres and temperature
void send_water_depth() const;
private:
void handle_message(const mavlink_message_t &msg) override;

4
Rover/Log.cpp
View File

@ -79,10 +79,6 @@ void Rover::Log_Write_Depth()
(double)(s->distance()),
temp_C);
}
#if AP_RANGEFINDER_ENABLED
// send water depth and temp to ground station
gcs().send_message(MSG_WATER_DEPTH);
#endif
}
#endif

49
Rover/ReleaseNotes.txt
View File

@ -1,6 +1,53 @@
Rover Release Notes:
------------------------------------------------------------------
Release 4.6.0-beta 13 Nov 2024
Release 4.6.0-beta2 11 Dec 2024
Changes from 4.6.0-beta1
1) Board specfic changes
- FoxeerF405v2 supports BMP280 baro
- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro
- MUPilot support
- SkySakura H743 support
- TBS Lucid H7 support
- VUAV-V7pro README documentation fixed
- X-MAV AP-H743v2 CAN pin definition fixed
2) Copter specific enhancements and bug fixes
- AutoTune fix for calc of maximum angular acceleration
- Advanced Failsafe customer build server option
3) Plane related enhancements and bug fixes
- QuadPlane fix for QLand getting stuck in pilot repositioning
- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards)
- Takeoff direction fixed when no yaw source
- TECS correctly handles home altitude changes
4) Bug Fixes and minor enhancements
- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor
- CAN frame logging added to ease support
- CRSF reconnection after failsafe fixed
- EKF3 position and velocity resets default to user defined source
- Ethernet IP address default 192.168.144.x
- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed)
- Fence pre-arm check that vehicle is within polygon fence
- Fence handling of more than 256 items fixed
- FFT protection against divide-by-zero in Jain estimator
- Frsky telemetry apparent wind speed fixed
- Inertial sensors stop sensor converging if motors arm
- Inertial sensors check for changes to notch filters fixed
- Real Time Clock allowed to shift forward when disarmed
- ROS2/DDS get/set parameter service added
- Scripting gets memory handling improvements
- Scripting promote video-stream-information to applet
- Topotek gimbal driver uses GIA message to retrieve current angle
- Tramp VTX OSD power indicator fixed
------------------------------------------------------------------
Release 4.6.0-beta1 13 Nov 2024
Changes from 4.5.7

8
Tools/AP_Bootloader/board_types.txt
View File

@ -296,6 +296,8 @@ AP_HW_CrazyF405 1177
AP_HW_FlywooF405HD_AIOv2 1180
AP_HW_FlywooH743Pro 1181
AP_HW_JFB200 1200
AP_HW_ESP32_PERIPH 1205
AP_HW_ESP32S3_PERIPH 1206
@ -331,6 +333,7 @@ AP_HW_MountainEagleH743 1444
AP_HW_StellarF4 1500
AP_HW_GEPRCF745BTHD 1501
AP_HW_GEPRC_TAKER_H743 1502
AP_HW_HGLRCF405V4 1524
@ -430,7 +433,10 @@ AP_HW_CUAV-7-NANO 7000
# IDs 7100-7109 reserved for V-UAV
AP_HW_VUAV-V7pro 7100
# please fill gaps in the above ranges rather than adding past ID #7109
# IDs 7110-7119 reserved for AEROFOX
AP_HW_AEROFOX_H7 7110
# please fill gaps in the above ranges rather than adding past ID #7199
# OpenDroneID enabled boards. Use 10000 + the base board ID

16
Tools/AP_Periph/imu.cpp
View File

@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal;
void AP_Periph_FW::can_imu_update(void)
{
while (true) {
// sleep for a bit to avoid flooding the CPU
hal.scheduler->delay_microseconds(100);
// we need to delay by a ms value as hal->schedule->delay_microseconds_boost
// used in wait_for_sample() takes uint16_t
const uint32_t delay_ms = 1000U / g.imu_sample_rate;
hal.scheduler->delay(delay_ms);
if (delay_ms == 0) {
// sleep for a bit to avoid flooding the CPU
hal.scheduler->delay_microseconds(100);
}
imu.update();
@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void)
pkt.accelerometer_latest[1] = tmp.y;
pkt.accelerometer_latest[2] = tmp.z;
tmp = imu.get_gyro();
pkt.rate_gyro_latest[0] = tmp.x;
pkt.rate_gyro_latest[1] = tmp.y;
pkt.rate_gyro_latest[2] = tmp.z;
uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE];
uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,

8
Tools/CPUInfo/CPUInfo.cpp
View File

@ -212,13 +212,13 @@ static void test_div1000(void)
for (uint32_t i=0; i<2000000; i++) {
uint64_t v = 0;
if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) {
AP_HAL::panic("ERROR: div1000 no random\n");
AP_HAL::panic("ERROR: div1000 no random");
break;
}
uint64_t v1 = v / 1000ULL;
uint64_t v2 = uint64_div1000(v);
if (v1 != v2) {
AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n",
AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx",
(unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2);
return;
}
@ -228,7 +228,7 @@ static void test_div1000(void)
for (uint32_t i=0; i<2000000; i++) {
uint64_t v = 0;
if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) {
AP_HAL::panic("ERROR: div1000 no random\n");
AP_HAL::panic("ERROR: div1000 no random");
break;
}
chSysLock();
@ -236,7 +236,7 @@ static void test_div1000(void)
uint64_t v2 = uint64_div1000(v);
chSysUnlock();
if (v1 != v2) {
AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n",
AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx",
(unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2);
return;
}

98
Tools/Frame_params/ModalAI/SentinelM0065.parm Normal file
View File

@ -0,0 +1,98 @@
# parameters for the Sentinel M0065 PWM ESC drone
# flight modes
FLTMODE1 0
FLTMODE2 0
FLTMODE3 0
FLTMODE4 2
FLTMODE5 0
FLTMODE6 0
FLTMODE_CH 6
# enable PID logging
LOG_BITMASK 65535
# mag field varies quite a lot between batteries
ARMING_MAGTHRESH 200
# IMU orientation
# 8 = ROLL 180
AHRS_ORIENTATION 8
# Forced external compass
COMPASS_EXTERNAL 2
# compass orientation
# ROTATION_ROLL_180_YAW_135 = 11
COMPASS_ORIENT 11
# filtering
INS_GYRO_FILTER 40
INS_HNTCH_ENABLE 1
INS_HNTCH_ATT 40.0
INS_HNTCH_BW 50.0
INS_HNTCH_FM_RAT 1.0
INS_HNTCH_FREQ 100.0
INS_HNTCH_HMNCS 7
INS_HNTCH_MODE 3
INS_HNTCH_OPTS 2
INS_HNTCH_REF 1.0
# run IMU at 2kHz
INS_GYRO_RATE 1
# a bit more agressive loiter
PILOT_SPEED_UP 500
LOIT_BRK_ACCEL 500
LOIT_BRK_JERK 1000
LOIT_BRK_DELAY 0.200000
# Tune parameters
ATC_RAT_PIT_D 0.002
ATC_RAT_PIT_I 0.08
ATC_RAT_PIT_P 0.08
ATC_RAT_RLL_D 0.002
ATC_RAT_RLL_I 0.08
ATC_RAT_RLL_P 0.08
# battery setup
BATT_LOW_VOLT 14.2
BATT_OPTIONS 64
BATT_VOLT_PIN 1
BATT_CURR_PIN 2
BATT_VOLT_MULT 1
BATT_AMP_PERVLT 1
BATT_AMP_OFFSET 0.0
# 4S battery range
MOT_BAT_VOLT_MAX 16.800000
MOT_BAT_VOLT_MIN 13.200000
# Learned hover thrust
# MOT_THST_EXPO 0.550000011920928955
# MOT_THST_HOVER 0.130549192428588867
# quad-X
FRAME_CLASS 1
# tweak R/C inputs
RC1_MIN 1000
RC1_MAX 2000
RC1_DZ 40
RC2_MIN 1000
RC2_MAX 2000
RC2_REVERSED 1
RC3_MIN 1000
RC3_MAX 2000
RC4_MIN 1000
RC4_MAX 2000
RC4_DZ 40
# add arming on right switch
RC7_OPTION 153
# Motor mappings
SERVO1_FUNCTION 33
SERVO2_FUNCTION 34
SERVO3_FUNCTION 35
SERVO4_FUNCTION 36

2
Tools/ardupilotwaf/boards.py
View File

@ -949,6 +949,7 @@ class sitl_periph_universal(sitl_periph):
HAL_PERIPH_ENABLE_AIRSPEED = 1,
HAL_PERIPH_ENABLE_MAG = 1,
HAL_PERIPH_ENABLE_BARO = 1,
HAL_PERIPH_ENABLE_IMU = 1,
HAL_PERIPH_ENABLE_RANGEFINDER = 1,
HAL_PERIPH_ENABLE_BATTERY = 1,
HAL_PERIPH_ENABLE_EFI = 1,
@ -963,6 +964,7 @@ class sitl_periph_universal(sitl_periph):
HAL_WITH_ESC_TELEM = 1,
AP_EXTENDED_ESC_TELEM_ENABLED = 1,
AP_TERRAIN_AVAILABLE = 1,
HAL_GYROFFT_ENABLED = 0,
)
class sitl_periph_gps(sitl_periph):

31
Tools/ardupilotwaf/esp32.py
View File

@ -57,6 +57,24 @@ def configure(cfg):
#env.append_value('GIT_SUBMODULES', 'esp_idf')
# delete the output sdkconfig file when the input defaults changes. we take the
# stamp as the output so we can compute the path to the sdkconfig, yet it
# doesn't have to exist when we're done.
class clean_sdkconfig(Task.Task):
def keyword(self):
return "delete sdkconfig generated from"
def run(self):
prefix = ".clean-stamp-"
for out in self.outputs:
if not out.name.startswith(prefix):
raise ValueError("not a stamp file: "+out)
dest = out.parent.abspath()+"/"+out.name[len(prefix):]
if os.path.exists(dest):
os.unlink(dest)
# waf needs the output to exist after the task, so touch it
open(out.abspath(), "w").close()
def pre_build(self):
"""Configure esp-idf as lib target"""
@ -74,8 +92,21 @@ def pre_build(self):
)
esp_idf_showinc = esp_idf.build('showinc', target='esp-idf_build/includes.list')
# task to delete the sdkconfig (thereby causing it to be regenerated) when
# the .defaults changes. it uses a stamp to find the sdkconfig. changing
# the sdkconfig WILL NOT cause it to be deleted as it's not an input. this
# is by design so the user can tweak it for testing purposes.
clean_sdkconfig_task = esp_idf_showinc.create_task("clean_sdkconfig",
src=self.srcnode.find_or_declare(self.env.AP_HAL_ESP32+"/sdkconfig.defaults"),
tgt=self.bldnode.find_or_declare("esp-idf_build/.clean-stamp-sdkconfig"))
esp_idf_showinc.post()
# ensure the sdkconfig will be deleted before the cmake configure occurs
# that regenerates it
esp_idf_showinc.cmake_config_task.set_run_after(clean_sdkconfig_task)
from waflib import Task
class load_generated_includes(Task.Task):
"""After includes.list generated include it in env"""

58
Tools/autotest/arducopter.py
View File

@ -1719,8 +1719,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
# Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance
def MinAltFenceAvoid(self):
'''Test Min Alt Fence Avoidance'''
self.takeoff(30, mode="LOITER")
"""Hold loiter position."""
# enable fence, only min altitude
# No action, rely on avoidance to prevent the breach
@ -1730,6 +1728,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
"FENCE_ALT_MIN": 20,
"FENCE_ACTION": 0,
})
self.reboot_sitl()
self.takeoff(30, mode="LOITER")
"""Hold loiter position."""
# Try and fly past the fence
self.set_rc(3, 1120)
@ -8304,6 +8306,57 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.do_RTL()
def TestTetherStuck(self):
"""Test tethered vehicle stuck because of tether"""
# Enable tether simulation
self.set_parameters({
"SIM_TETH_ENABLE": 1,
})
self.delay_sim_time(2)
self.reboot_sitl()
# Set tether line length
self.set_parameters({
"SIM_TETH_LINELEN": 10,
})
self.delay_sim_time(2)
# Prepare and take off
self.wait_ready_to_arm()
self.arm_vehicle()
self.takeoff(10, mode='LOITER')
# Simulate vehicle getting stuck by increasing RC throttle
self.set_rc(3, 1900)
self.delay_sim_time(5, reason='let tether get stuck')
# Monitor behavior for 10 seconds
tstart = self.get_sim_time()
initial_alt = self.get_altitude()
stuck = True # Assume it's stuck unless proven otherwise
while self.get_sim_time() - tstart < 10:
# Fetch current altitude
current_alt = self.get_altitude()
self.progress(f"current_alt={current_alt}")
# Fetch and log battery status
battery_status = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)
if battery_status:
self.progress(f"Battery: {battery_status}")
# Check if the vehicle is stuck.
# We assume the vehicle is stuck if the current is high and the altitude is not changing
if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2):
stuck = False # Vehicle moved or current is abnormal
break
if not stuck:
raise NotAchievedException("Vehicle did not get stuck as expected")
# Land and disarm the vehicle to ensure we can go down
self.land_and_disarm()
def fly_rangefinder_drivers_fly(self, rangefinders):
'''ensure rangefinder gives height-above-ground'''
self.change_mode('GUIDED')
@ -12330,6 +12383,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.MAV_CMD_MISSION_START_p1_p2,
self.ScriptingAHRSSource,
self.CommonOrigin,
self.TestTetherStuck,
])
return ret

174
Tools/autotest/arduplane.py
View File

@ -3661,7 +3661,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
def FenceAutoEnableDisableSwitch(self):
'''Tests autoenablement of regular fences and manual disablement'''
self.set_parameters({
"FENCE_TYPE": 11, # Set fence type to min alt
"FENCE_TYPE": 9, # Set fence type to min alt, max alt
"FENCE_ACTION": 1, # Set action to RTL
"FENCE_ALT_MIN": 50,
"FENCE_ALT_MAX": 100,
@ -3672,44 +3672,88 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"FENCE_RET_ALT" : 0,
"FENCE_RET_RALLY" : 0,
"FENCE_TOTAL" : 0,
"RTL_ALTITUDE" : 75,
"TKOFF_ALT" : 75,
"RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality
})
self.reboot_sitl()
self.context_collect("STATUSTEXT")
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
# Grab Home Position
self.mav.recv_match(type='HOME_POSITION', blocking=True)
self.set_rc_from_map({7: 1000}) # Turn fence off with aux function
self.set_rc(7, 1000) # Turn fence off with aux function, does not impact later auto-enable
self.wait_ready_to_arm()
self.progress("Check fence disabled at boot")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence is enabled at boot")
cruise_alt = 75
self.takeoff(cruise_alt, mode='TAKEOFF')
self.progress("Fly above ceiling and check there is no breach")
self.progress("Fly above ceiling and check there is a breach")
self.change_mode('FBWA')
self.set_rc(3, 2000)
self.change_altitude(cruise_alt + 80, relative=True)
self.set_rc(2, 1000)
self.wait_statustext("Max Alt fence breached", timeout=10, check_context=True)
self.wait_mode('RTL')
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence Ceiling breached")
if (m.onboard_control_sensors_health & fence_bit):
raise NotAchievedException("Fence ceiling not breached")
self.set_rc(3, 1500)
self.set_rc(2, 1500)
self.progress("Wait for RTL alt reached")
self.wait_altitude(cruise_alt-5, cruise_alt+5, relative=True, timeout=30)
self.progress("Return to cruise alt")
self.set_rc(3, 1500)
self.change_altitude(cruise_alt, relative=True)
self.progress("Fly below floor and check for no breach")
self.change_altitude(25, relative=True)
self.progress("Check fence breach cleared")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence Ceiling breached")
raise NotAchievedException("Fence breach not cleared")
self.progress("Fly above floor and check fence is not re-enabled")
self.progress("Fly below floor and check for breach")
self.set_rc(2, 2000)
self.wait_statustext("Min Alt fence breached", timeout=10, check_context=True)
self.wait_mode("RTL")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (m.onboard_control_sensors_health & fence_bit):
raise NotAchievedException("Fence floor not breached")
self.change_mode("FBWA")
self.progress("Fly above floor and check fence is enabled")
self.set_rc(3, 2000)
self.change_altitude(75, relative=True)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_enabled & fence_bit)):
raise NotAchievedException("Fence Floor not enabled")
self.progress("Toggle fence enable/disable")
self.set_rc(7, 2000)
self.delay_sim_time(2)
self.set_rc(7, 1000)
self.delay_sim_time(2)
self.progress("Check fence is disabled")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence Ceiling re-enabled")
raise NotAchievedException("Fence disable with switch failed")
self.progress("Fly below floor and check for no breach")
self.change_altitude(40, relative=True)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence floor breached")
self.progress("Return to cruise alt")
self.set_rc(3, 1500)
@ -4805,6 +4849,41 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.fly_home_land_and_disarm()
def TakeoffTakeoff5(self):
'''Test the behaviour of a takeoff with no compass'''
self.set_parameters({
"COMPASS_USE": 0,
"COMPASS_USE2": 0,
"COMPASS_USE3": 0,
})
import copy
start_loc = copy.copy(SITL_START_LOCATION)
start_loc.heading = 175
self.customise_SITL_commandline(["--home=%.9f,%.9f,%.2f,%.1f" % (
start_loc.lat, start_loc.lng, start_loc.alt, start_loc.heading)])
self.reboot_sitl()
self.change_mode("TAKEOFF")
# waiting for the EKF to be happy won't work
self.delay_sim_time(20)
self.arm_vehicle()
target_alt = self.get_parameter("TKOFF_ALT")
self.wait_altitude(target_alt-5, target_alt, relative=True)
# Wait a bit for the Takeoff altitude to settle.
self.delay_sim_time(5)
bearing_margin = 35
loc = self.mav.location()
bearing_from_home = self.get_bearing(start_loc, loc)
if bearing_from_home < 0:
bearing_from_home += 360
if abs(bearing_from_home - start_loc.heading) > bearing_margin:
raise NotAchievedException(f"Did not takeoff in the right direction {bearing_from_home}")
self.fly_home_land_and_disarm()
def TakeoffGround(self):
'''Test a rolling TAKEOFF.'''
@ -5326,6 +5405,40 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
else:
raise NotAchievedException("Missing trick %s" % t)
def UniversalAutoLandScript(self):
'''Test UniversalAutoLandScript'''
applet_script = "UniversalAutoLand.lua"
self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"])
self.install_applet_script_context(applet_script)
self.context_collect('STATUSTEXT')
self.set_parameters({
"SCR_ENABLE" : 1,
"SCR_VM_I_COUNT" : 1000000,
"RTL_AUTOLAND" : 2
})
self.reboot_sitl()
self.wait_text("Loaded UniversalAutoLand.lua", check_context=True)
self.set_parameters({
"AUTOLAND_ENABLE" : 1,
"AUTOLAND_WP_ALT" : 55,
"AUTOLAND_WP_DIST" : 400
})
self.scripting_restart()
self.wait_text("Scripting: restarted", check_context=True)
self.wait_ready_to_arm()
self.arm_vehicle()
self.change_mode("AUTO")
self.wait_text("Captured initial takeoff direction", check_context=True)
self.wait_disarmed(120)
self.progress("Check the landed heading matches takeoff")
self.wait_heading(173, accuracy=5, timeout=1)
loc = mavutil.location(-35.362938, 149.165085, 585, 173)
if self.get_distance(loc, self.mav.location()) > 35:
raise NotAchievedException("Did not land close to home")
def SDCardWPTest(self):
'''test BRD_SD_MISSION support'''
spiral_script = "mission_spiral.lua"
@ -6225,14 +6338,18 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
def SetHomeAltChange(self):
'''check modes retain altitude when home alt changed'''
for mode in 'FBWB', 'CRUISE', 'LOITER':
self.set_rc(3, 1000)
self.wait_ready_to_arm()
home = self.home_position_as_mav_location()
self.takeoff(20)
higher_home = home
target_alt = 20
self.takeoff(target_alt, mode="TAKEOFF")
self.delay_sim_time(20) # Give some time to altitude to stabilize.
self.set_rc(3, 1500)
self.change_mode(mode)
higher_home = copy.copy(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.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=10, timeout=11)
self.disarm_vehicle(force=True)
self.reboot_sitl()
@ -6268,6 +6385,23 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.disarm_vehicle(force=True)
self.reboot_sitl()
def SetHomeAltChange3(self):
'''same as SetHomeAltChange, but the home alt change occurs during TECS operation'''
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.
higher_home = copy.copy(home)
higher_home.alt += 40
self.set_home(higher_home)
self.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=10, timeout=10.1)
self.disarm_vehicle(force=True)
self.reboot_sitl()
def ForceArm(self):
'''check force-arming functionality'''
self.set_parameter("SIM_GPS1_ENABLE", 0)
@ -6457,6 +6591,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.Soaring,
self.Terrain,
self.TerrainMission,
self.UniversalAutoLandScript,
])
return ret
@ -6490,6 +6625,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.TakeoffTakeoff2,
self.TakeoffTakeoff3,
self.TakeoffTakeoff4,
self.TakeoffTakeoff5,
self.TakeoffGround,
self.TakeoffIdleThrottle,
self.TakeoffBadLevelOff,
@ -6538,6 +6674,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.GPSPreArms,
self.SetHomeAltChange,
self.SetHomeAltChange2,
self.SetHomeAltChange3,
self.ForceArm,
self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
self.GliderPullup,
@ -6549,7 +6686,6 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"LandingDrift": "Flapping test. See https://github.com/ArduPilot/ardupilot/issues/20054",
"InteractTest": "requires user interaction",
"ClimbThrottleSaturation": "requires https://github.com/ArduPilot/ardupilot/pull/27106 to pass",
"SetHomeAltChange": "https://github.com/ArduPilot/ardupilot/issues/5672",
}

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

@ -77,7 +77,7 @@ 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_RAT_YAW_I 0.02
Q_A_ANG_RLL_P 6
Q_A_ANG_PIT_P 6
RTL_ALTITUDE 20.00

2
Tools/autotest/test_build_options.py
View File

@ -268,6 +268,8 @@ class TestBuildOptions(object):
'AP_OPTICALFLOW_ONBOARD_ENABLED', # only instantiated on Linux
'HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL', # entirely elided if no user
'AP_PLANE_BLACKBOX_LOGGING', # entirely elided if no user
'AP_COMPASS_AK8963_ENABLED', # probed on a board-by-board basis, not on CubeOrange for example
'AP_COMPASS_LSM303D_ENABLED', # probed on a board-by-board basis, not on CubeOrange for example
])
if target.lower() != "copter":
feature_define_whitelist.add('MODE_ZIGZAG_ENABLED')

9
Tools/autotest/vehicle_test_suite.py
View File

@ -1715,8 +1715,8 @@ class FRSkySPort(FRSky):
if not self.connect():
self.progress("Failed to connect")
return
self.check_poll()
self.do_sport_read()
self.check_poll()
def do_sport_read(self):
self.buffer += self.do_read()
@ -13738,8 +13738,11 @@ switch value'''
def FRSkySPort(self):
'''Test FrSky SPort mode'''
self.set_parameter("SERIAL5_PROTOCOL", 4) # serial5 is FRSky sport
self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor
self.set_parameters({
"SERIAL5_PROTOCOL": 4, # serial5 is FRSky sport
"RPM1_TYPE": 10, # enable SITL RPM sensor
"GPS1_TYPE": 100, # use simulated backend for consistent position
})
port = self.spare_network_port()
self.customise_SITL_commandline([
"--serial5=tcp:%u" % port # serial5 spews to localhost port

BIN
Tools/bootloaders/AEROFOX-H7_bl.bin generated Normal file
View File

Binary file not shown.

BIN
Tools/bootloaders/GEPRC_TAKER_H743_bl.bin generated Normal file
View File

Binary file not shown.

1168
Tools/bootloaders/GEPRC_TAKER_H743_bl.hex generated Normal file
View File

File diff suppressed because it is too large Load Diff

1
Tools/environment_install/install-prereqs-alpine.sh
View File

@ -17,7 +17,6 @@ apk update && apk add --no-cache \
libxml2-dev \
libxslt-dev \
git \
&& ln -sf python3 /usr/bin/python \
&& rm -rf /var/cache/apk/*
python3 -m pip install --user --no-deps --no-cache-dir empy==3.3.4 pexpect ptyprocess --break-system-packages

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

@ -120,6 +120,11 @@ elif [ ${RELEASE_CODENAME} == 'noble' ]; then
SITLCFML_VERSION="2.6"
PYTHON_V="python3"
PIP=pip3
elif [ ${RELEASE_CODENAME} == 'oracular' ]; then
SITLFML_VERSION="2.6"
SITLCFML_VERSION="2.6"
PYTHON_V="python3"
PIP=pip3
elif [ ${RELEASE_CODENAME} == 'groovy' ] ||
[ ${RELEASE_CODENAME} == 'bullseye' ]; then
SITLFML_VERSION="2.5"
@ -176,7 +181,8 @@ ARM_LINUX_PKGS="g++-arm-linux-gnueabihf $INSTALL_PKG_CONFIG"
if [ ${RELEASE_CODENAME} == 'bookworm' ] ||
[ ${RELEASE_CODENAME} == 'lunar' ] ||
[ ${RELEASE_CODENAME} == 'mantic' ] ||
[ ${RELEASE_CODENAME} == 'noble' ]; then
[ ${RELEASE_CODENAME} == 'noble' ] ||
[ ${RELEASE_CODENAME} == 'oracular' ]; then
# on Lunar (and presumably later releases), we install in venv, below
PYTHON_PKGS+=" numpy pyparsing psutil"
SITL_PKGS="python3-dev"
@ -189,7 +195,8 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
if [ ${RELEASE_CODENAME} == 'bookworm' ] ||
[ ${RELEASE_CODENAME} == 'lunar' ] ||
[ ${RELEASE_CODENAME} == 'mantic' ] ||
[ ${RELEASE_CODENAME} == 'noble' ]; then
[ ${RELEASE_CODENAME} == 'noble' ] ||
[ ${RELEASE_CODENAME} == 'oracular' ]; then
PYTHON_PKGS+=" matplotlib scipy opencv-python pyyaml"
SITL_PKGS+=" xterm libcsfml-dev libcsfml-audio${SITLCFML_VERSION} libcsfml-dev libcsfml-graphics${SITLCFML_VERSION} libcsfml-network${SITLCFML_VERSION} libcsfml-system${SITLCFML_VERSION} libcsfml-window${SITLCFML_VERSION} libsfml-audio${SITLFML_VERSION} libsfml-dev libsfml-graphics${SITLFML_VERSION} libsfml-network${SITLFML_VERSION} libsfml-system${SITLFML_VERSION} libsfml-window${SITLFML_VERSION}"
else
@ -283,8 +290,9 @@ elif [ ${RELEASE_CODENAME} == 'lunar' ]; then
elif [ ${RELEASE_CODENAME} == 'buster' ]; then
SITL_PKGS+=" libpython3-stdlib" # for argparse
elif [ ${RELEASE_CODENAME} != 'mantic' ] &&
[ ${RELEASE_CODENAME} != 'noble' ]; then
SITL_PKGS+=" python-argparse"
[ ${RELEASE_CODENAME} != 'noble' ] &&
[ ${RELEASE_CODENAME} != 'oracular' ]; then
SITL_PKGS+=" python-argparse"
fi
# Check for graphical package for MAVProxy
@ -305,6 +313,9 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
elif [ ${RELEASE_CODENAME} == 'noble' ]; then
SITL_PKGS+=" libgtk-3-dev libwxgtk3.2-dev "
# see below
elif [ ${RELEASE_CODENAME} == 'oracular' ]; then
SITL_PKGS+=" libgtk-3-dev libwxgtk3.2-dev "
# see below
elif apt-cache search python-wxgtk3.0 | grep wx; then
SITL_PKGS+=" python-wxgtk3.0"
elif apt-cache search python3-wxgtk4.0 | grep wx; then
@ -325,7 +336,8 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
SITL_PKGS+=" python3-wxgtk4.0"
SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame
elif [ ${RELEASE_CODENAME} == 'mantic' ] ||
[ ${RELEASE_CODENAME} == 'noble' ]; then
[ ${RELEASE_CODENAME} == 'noble' ] ||
[ ${RELEASE_CODENAME} == 'oracular' ]; then
PYTHON_PKGS+=" wxpython opencv-python"
SITL_PKGS+=" python3-wxgtk4.0"
SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame
@ -380,6 +392,8 @@ if [ ${RELEASE_CODENAME} == 'bookworm' ] ||
PYTHON_VENV_PACKAGE=python3.11-venv
elif [ ${RELEASE_CODENAME} == 'noble' ]; then
PYTHON_VENV_PACKAGE=python3.12-venv
elif [ ${RELEASE_CODENAME} == 'oracular' ]; then
PYTHON_VENV_PACKAGE=python3.12-venv
fi
if [ -n "$PYTHON_VENV_PACKAGE" ]; then
@ -416,7 +430,8 @@ fi
if [ ${RELEASE_CODENAME} == 'bookworm' ] ||
[ ${RELEASE_CODENAME} == 'lunar' ] ||
[ ${RELEASE_CODENAME} == 'mantic' ] ||
[ ${RELEASE_CODENAME} == 'noble' ]; then
[ ${RELEASE_CODENAME} == 'noble' ] ||
[ ${RELEASE_CODENAME} == 'oracular' ]; then
# must do this ahead of wxPython pip3 run :-/
$PIP install $PIP_USER_ARGUMENT -U attrdict3
fi

2
Tools/environment_install/install-prereqs-windows-andAPMSource.ps1
View File

@ -15,8 +15,6 @@ Start-BitsTransfer -Source "https://firmware.ardupilot.org/Tools/STM32-tools/gcc
Write-Output "Installing Cygwin x64 (4/8)"
Start-Process -wait -FilePath $PSScriptRoot\setup-x86_64.exe -ArgumentList "--root=C:\cygwin64 --no-startmenu --local-package-dir=$env:USERPROFILE\Downloads --site=http://cygwin.mirror.constant.com --packages autoconf,automake,ccache,cygwin32-gcc-g++,gcc-g++=7.4.0-1,libgcc1=7.4.0.1,gcc-core=7.4.0-1,git,libtool,make,gawk,libexpat-devel,libxml2-devel,python37,python37-future,python37-lxml,python37-pip,libxslt-devel,python37-devel,procps-ng,zip,gdb,ddd --quiet-mode"
Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/python3.7 /usr/bin/python'"
Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/pip3.7 /usr/bin/pip'"
Write-Output "Downloading extra Python packages (5/8)"
Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'python3.7 -m pip install empy==3.3.4 pyserial pymavlink intelhex dronecan pexpect'"

2
Tools/environment_install/install-prereqs-windows.ps1
View File

@ -15,8 +15,6 @@ Start-BitsTransfer -Source "https://firmware.ardupilot.org/Tools/STM32-tools/gcc
Write-Output "Installing Cygwin x64 (4/7)"
Start-Process -wait -FilePath $PSScriptRoot\setup-x86_64.exe -ArgumentList "--root=C:\cygwin64 --no-startmenu --local-package-dir=$env:USERPROFILE\Downloads --site=http://cygwin.mirror.constant.com --packages autoconf,automake,ccache,cygwin32-gcc-g++,gcc-g++=7.4.0-1,libgcc1=7.4.0.1,gcc-core=7.4.0-1,git,libtool,make,gawk,libexpat-devel,libxml2-devel,python37,python37-future,python37-lxml,python37-pip,libxslt-devel,python37-devel,procps-ng,zip,gdb,ddd,xterm --quiet-mode"
Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/python3.7 /usr/bin/python'"
Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/pip3.7 /usr/bin/pip'"
Write-Output "Downloading extra Python packages (5/7)"
Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'python3.7 -m pip install empy==3.3.4 pyserial pymavlink intelhex dronecan pexpect'"

43
Tools/scripts/CAN/CAN_playback.py
View File

@ -1,6 +1,8 @@
#!/usr/bin/env python3
'''
playback a set of CAN frames from libraries/AP_Scripting/examples/CAN_logger.lua onto a CAN bus
playback a set of CAN frames
capture frames either using libraries/AP_Scripting/examples/CAN_logger.lua
or the CAN_Pn_OPTIONS bit to enable CAN logging
'''
import dronecan
@ -9,6 +11,7 @@ import sys
import threading
from pymavlink import mavutil
from dronecan.driver.common import CANFrame
import struct
# get command line arguments
@ -16,6 +19,7 @@ from argparse import ArgumentParser
parser = ArgumentParser(description='CAN playback')
parser.add_argument("logfile", default=None, type=str, help="logfile")
parser.add_argument("canport", default=None, type=str, help="CAN port")
parser.add_argument("--bus", default=0, type=int, help="CAN bus")
args = parser.parse_args()
@ -28,8 +32,27 @@ mlog = mavutil.mavlink_connection(args.logfile)
tstart = time.time()
first_tstamp = None
def dlc_to_datalength(dlc):
# Data Length Code 9 10 11 12 13 14 15
# Number of data bytes 12 16 20 24 32 48 64
if (dlc <= 8):
return dlc
elif (dlc == 9):
return 12
elif (dlc == 10):
return 16
elif (dlc == 11):
return 20
elif (dlc == 12):
return 24
elif (dlc == 13):
return 32
elif (dlc == 14):
return 48
return 64
while True:
m = mlog.recv_match(type='CANF')
m = mlog.recv_match(type=['CANF','CAFD'])
if m is None:
print("Rewinding")
@ -38,15 +61,25 @@ while True:
first_tstamp = None
continue
if getattr(m,'bus',0) != args.bus:
continue
if first_tstamp is None:
first_tstamp = m.TimeUS
dt = time.time() - tstart
dt2 = (m.TimeUS - first_tstamp)*1.0e-6
if dt2 > dt:
time.sleep(dt2 - dt)
data = [m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7]
data = data[:m.DLC]
canfd = m.get_type() == 'CAFD'
if canfd:
data = struct.pack("<QQQQQQQQ", m.D0, m.D1, m.D2, m.D3, m.D4, m.D5, m.D6, m.D7)
data = data[:dlc_to_datalength(m.DLC)]
else:
data = struct.pack("<BBBBBBBB", m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7)
data = data[:m.DLC]
fid = m.Id
is_extended = (fid & (1<<31)) != 0
driver.send(fid, data, extended=is_extended, canfd=False)
driver.send(fid, data, extended=is_extended, canfd=canfd)
print(m)

6
Tools/scripts/build_options.py
View File

@ -146,7 +146,7 @@ BUILD_OPTIONS = [
Feature('Camera', 'Camera_ThermalRange', 'AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'Enable GCS camera thermal range', 0, 'Camera,MOUNT'), # noqa: E501
Feature('Camera', 'Camera_Info_From_Script', 'AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'Enable Camera information messages via Lua script', 0, 'Camera,SCRIPTING'), # noqa
Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam control', 0, None),
Feature('Camera', 'RUNCAM', 'AP_CAMERA_RUNCAM_ENABLED', 'Enable RunCam control', 0, None),
Feature('Copter', 'MODE_ZIGZAG', 'MODE_ZIGZAG_ENABLED', 'Enable Mode ZigZag', 0, None),
Feature('Copter', 'MODE_SYSTEMID', 'MODE_SYSTEMID_ENABLED', 'Enable Mode SystemID', 0, 'Logging'),
@ -358,6 +358,7 @@ BUILD_OPTIONS = [
Feature('Other', 'Buttons', 'HAL_BUTTON_ENABLED', 'Enable Buttons', 0, None),
Feature('Other', 'Logging', 'HAL_LOGGING_ENABLED', 'Enable Logging', 0, None),
Feature('Other', 'CUSTOM_ROTATIONS', 'AP_CUSTOMROTATIONS_ENABLED', 'Enable Custom sensor rotations', 0, None),
Feature('Other', 'PID_FILTERING', 'AP_FILTER_ENABLED', 'Enable PID filtering', 0, None),
# MAVLink section for mavlink features and/or message handling,
# rather than for e.g. mavlink-based sensor drivers
@ -433,7 +434,8 @@ BUILD_OPTIONS = [
Feature('Networking', 'PPP', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None),
Feature('Networking', 'CAN MCAST', 'AP_NETWORKING_CAN_MCAST_ENABLED', 'Enable CAN multicast bridge', 0, None),
Feature('DroneCAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None),
Feature('CAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None),
Feature('CAN', 'CAN Logging', 'AP_CAN_LOGGING_ENABLED', 'Enable CAN logging support', 0, None),
]
BUILD_OPTIONS.sort(key=lambda x: (x.category + x.label))

4
Tools/scripts/extract_features.py
View File

@ -138,7 +138,7 @@ class ExtractFeatures(object):
('AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'AP_Camera::send_camera_fov_status'),
('AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'AP_Camera::send_camera_thermal_range'),
('AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'AP_Camera_Backend::set_camera_information'),
('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',),
('AP_CAMERA_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',),
('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',),
('AP_PROXIMITY_{type}_ENABLED', 'AP_Proximity_(?P<type>.*)::update',),
@ -284,6 +284,8 @@ 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'),
('AP_FILTER_ENABLED', r'AP_Filters::update'),
('AP_CAN_LOGGING_ENABLED', r'AP_CANManager::can_logging_callback'),
]
def progress(self, msg):

1
Tools/scripts/generate_manifest.py
View File

@ -107,6 +107,7 @@ brand_map = {
"MicoAir405v2" : ("MicoAir F405 v2.1", "MicoAir"),
"MicoAir405Mini" : ("MicoAir F405 Mini", "MicoAir"),
"GEPRCF745BTHD": ("TAKER F745 BT","GEPRC"),
"GEPRC_TAKER_H743": ("TAKER H743 BT","GEPRC"),
}
class Firmware():

2
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -644,7 +644,7 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl
}
// twitching_measure_acceleration - measure rate of change of measurement
void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const
void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const
{
if (rate_max < rate) {
rate_max = rate;

2
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -156,7 +156,7 @@ private:
void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max);
// measure acceleration during twitch test
void twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const;
void twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const;
// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back
// optimize D term while keeping the maximum just below the target by adjusting P

1
libraries/AC_Avoidance/AC_Avoidance_Logging.cpp
View File

@ -6,6 +6,7 @@
#include "AP_OADijkstra.h"
#include "AP_OABendyRuler.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_OAPATHPLANNER_BENDYRULER_ENABLED
void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const

80
libraries/AC_Fence/AC_Fence.cpp
View File

@ -219,7 +219,6 @@ void AC_Fence::update()
// if someone changes the parameter we want to enable or disable everything
if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) {
// reset the auto mask since we just reconfigured all of fencing
_auto_enable_mask = AC_FENCE_ALL_FENCES;
_last_enabled = _enabled;
_last_auto_enabled = _auto_enabled;
if (_enabled) {
@ -238,9 +237,9 @@ void AC_Fence::update()
}
// enable or disable configured fences present in fence_types
// also updates the bitmask of auto enabled fences if update_auto_mask is true
// also updates the _min_alt_state enum if update_auto_enable is true
// returns a bitmask of fences that were changed
uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable)
{
uint8_t fences = _configured_fences.get() & fence_types;
uint8_t enabled_fences = _enabled_fences;
@ -250,9 +249,9 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
enabled_fences &= ~fences;
}
// fences that were manually changed are no longer eligible for auto-enablement or disablement
if (update_auto_mask) {
_auto_enable_mask &= ~fences;
if (update_auto_enable && (fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
// remember that min-alt fence was manually enabled/disabled
_min_alt_state = value ? MinAltState::MANUALLY_ENABLED : MinAltState::MANUALLY_DISABLED;
}
uint8_t fences_to_change = _enabled_fences ^ enabled_fences;
@ -260,6 +259,7 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
if (!fences_to_change) {
return 0;
}
#if HAL_LOGGING_ENABLED
AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE);
if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) {
@ -305,7 +305,11 @@ void AC_Fence::auto_enable_fence_on_arming(void)
return;
}
const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false);
// reset min alt state, after an auto-enable the min alt fence can be auto-enabled on
// reaching altitude
_min_alt_state = MinAltState::DEFAULT;
const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false);
print_fence_message("auto-enabled", fences);
}
@ -318,7 +322,7 @@ void AC_Fence::auto_disable_fence_on_disarming(void)
return;
}
const uint8_t fences = enable(false, _auto_enable_mask, false);
const uint8_t fences = enable(false, AC_FENCE_ALL_FENCES, false);
print_fence_message("auto-disabled", fences);
}
@ -332,7 +336,10 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
return;
}
const uint8_t fences = enable(true, _auto_enable_mask, false);
// reset min-alt state
_min_alt_state = MinAltState::DEFAULT;
const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false);
print_fence_message("auto-enabled", fences);
}
@ -342,14 +349,18 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const
uint8_t auto_disable = 0;
switch (auto_enabled()) {
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
auto_disable = _auto_enable_mask;
auto_disable = AC_FENCE_ALL_FENCES;
break;
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
default: // when auto disable is not set we still need to disable the altmin fence on landing
auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN;
auto_disable = AC_FENCE_TYPE_ALT_MIN;
break;
}
if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
// don't auto-disable min alt fence if manually enabled
auto_disable &= ~AC_FENCE_TYPE_ALT_MIN;
}
return auto_disable;
}
@ -469,8 +480,20 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c
return false;
}
auto breached_fences = _breached_fences;
if (auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
Location loc;
if (!AP::ahrs().get_location(loc)) {
hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position");
return false;
}
if (_poly_loader.breached(loc)) {
breached_fences |= AC_FENCE_TYPE_POLYGON;
}
}
// check no limits are currently breached
if (_breached_fences) {
if (breached_fences) {
char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
AC_Fence::get_fence_names(_breached_fences, e);
@ -511,7 +534,7 @@ bool AC_Fence::check_fence_alt_max()
float alt;
AP::ahrs().get_relative_position_D_home(alt);
_curr_alt = -alt; // translate Down to Up
const float _curr_alt = -alt; // translate Down to Up
// check if we are over the altitude fence
if (_curr_alt >= _alt_max) {
@ -560,7 +583,7 @@ bool AC_Fence::check_fence_alt_min()
float alt;
AP::ahrs().get_relative_position_D_home(alt);
_curr_alt = -alt; // translate Down to Up
const float _curr_alt = -alt; // translate Down to Up
// check if we are under the altitude fence
if (_curr_alt <= _alt_min) {
@ -603,7 +626,7 @@ bool AC_Fence::auto_enable_fence_floor()
// altitude fence check
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured
|| (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled
|| !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled
|| _min_alt_state == MinAltState::MANUALLY_DISABLED // user has manually disabled the fence
|| (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
|| auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
// not enabled
@ -612,7 +635,7 @@ bool AC_Fence::auto_enable_fence_floor()
float alt;
AP::ahrs().get_relative_position_D_home(alt);
_curr_alt = -alt; // translate Down to Up
const float _curr_alt = -alt; // translate Down to Up
// check if we are over the altitude fence
if (!floor_enabled() && _curr_alt >= _alt_min + _margin) {
@ -709,11 +732,36 @@ uint8_t AC_Fence::check(bool disable_auto_fences)
// clear any breach from disabled fences
clear_breach(fences_to_disable);
if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
// if user has manually enabled the min-alt fence then don't auto-disable
fences_to_disable &= ~AC_FENCE_TYPE_ALT_MIN;
}
// report on any fences that were auto-disabled
if (fences_to_disable) {
print_fence_message("auto-disabled", fences_to_disable);
}
#if 0
/*
this debug log message is very useful both when developing tests
and doing manual SITL fence testing
*/
{
float alt;
AP::ahrs().get_relative_position_D_home(alt);
AP::logger().WriteStreaming("FENC", "TimeUS,EN,AE,CF,EF,DF,Alt", "QIIIIIf",
AP_HAL::micros64(),
enabled(),
_auto_enabled,
_configured_fences,
get_enabled_fences(),
disabled_fences,
alt*-1);
}
#endif
// return immediately if disabled
if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
return 0;

10
libraries/AC_Fence/AC_Fence.h
View File

@ -249,10 +249,7 @@ private:
float _circle_breach_distance; // distance beyond the circular fence
// other internal variables
uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled
float _home_distance; // distance from home in meters (provided by main code)
float _curr_alt;
// breach information
uint8_t _breached_fences; // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
@ -262,6 +259,13 @@ private:
uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control
enum class MinAltState
{
DEFAULT = 0,
MANUALLY_ENABLED,
MANUALLY_DISABLED
} _min_alt_state;
AC_PolyFence_loader _poly_loader{_total, _options}; // polygon fence
};

39
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -172,7 +172,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @Param: OPTIONS
// @DisplayName: ADS-B Options
// @Description: Options for emergency failsafe codes and device capabilities
// @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe,3:Sagetech MXS use External Config
// @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe,3:Sagetech MXS use External Config,4:Transmit in traditional Mode 3A/C only and inhibit Mode-S and ES (ADSB) transmissions
// @User: Advanced
AP_GROUPINFO("OPTIONS", 15, AP_ADSB, _options, 0),
@ -697,7 +697,6 @@ void AP_ADSB::handle_out_control(const mavlink_uavionix_adsb_out_control_t &pack
{
out_state.ctrl.baroCrossChecked = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_EXTERNAL_BARO_CROSSCHECKED;
out_state.ctrl.airGroundState = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_ON_GROUND;
out_state.ctrl.identActive = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_IDENT_BUTTON_ACTIVE;
out_state.ctrl.modeAEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_A_ENABLED;
out_state.ctrl.modeCEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_C_ENABLED;
out_state.ctrl.modeSEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_S_ENABLED;
@ -707,6 +706,10 @@ void AP_ADSB::handle_out_control(const mavlink_uavionix_adsb_out_control_t &pack
out_state.ctrl.emergencyState = packet.emergencyStatus;
memcpy(out_state.ctrl.callsign, packet.flight_id, sizeof(out_state.ctrl.callsign));
out_state.ctrl.x_bit = packet.x_bit;
if (packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_IDENT_BUTTON_ACTIVE) {
IGNORE_RETURN(ident_start());
}
}
/*
@ -930,6 +933,18 @@ uint32_t AP_ADSB::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNu
return outputNumber;
}
// Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller.
// See wikipedia for IDENT explanation https://en.wikipedia.org/wiki/Transponder_(aeronautics)
bool AP_ADSB::ident_start()
{
if (!healthy()) {
return false;
}
out_state.ctrl.identActive = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"ADSB: IDENT!");
return true;
}
// methods for embedded class Location
bool AP_ADSB::Loc::speed_accuracy(float &sacc) const
{
@ -958,6 +973,26 @@ bool AP_ADSB::Loc::vertical_accuracy(float &vacc) const
return true;
}
uint8_t AP_ADSB::convert_maxknots_to_enum(const float maxAircraftSpeed_knots)
{
if (maxAircraftSpeed_knots <= 0) {
// not set or unknown. no bits set
return 0;
} else if (maxAircraftSpeed_knots <= 75) {
return 1;
} else if (maxAircraftSpeed_knots <= 150) {
return 2;
} else if (maxAircraftSpeed_knots <= 300) {
return 3;
} else if (maxAircraftSpeed_knots <= 600) {
return 4;
} else if (maxAircraftSpeed_knots <= 1200) {
return 5;
} else {
return 6;
}
}
AP_ADSB *AP::ADSB()
{
return AP_ADSB::get_singleton();

11
libraries/AP_ADSB/AP_ADSB.h
View File

@ -78,6 +78,7 @@ public:
Squawk_7400_FS_RC = (1<<1),
Squawk_7400_FS_GCS = (1<<2),
SagteTech_MXS_External_Config = (1<<3),
Mode3_Only = (1<<4),
};
// for holding parameters
@ -207,6 +208,8 @@ public:
// confirm a value is a valid callsign
static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED;
static uint8_t convert_maxknots_to_enum(const float maxAircraftSpeed_knots);
// Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value
// stored on a GCS as a string field in different format, but then transmitted
// over mavlink as a float which is always a decimal.
@ -214,13 +217,7 @@ public:
// Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller.
// See wikipedia for IDENT explanation https://en.wikipedia.org/wiki/Transponder_(aeronautics)
bool ident_start() {
if (!healthy() || ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) == 0)) {
return false;
}
out_state.ctrl.identActive = true;
return true;
}
bool ident_start();
AP_ADSB::Type get_type(uint8_t instance) const;

8
libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp
View File

@ -120,7 +120,7 @@ void AP_ADSB_Sagetech_MXS::update()
(mxs_state.inst.icao != (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get() ||
mxs_state.inst.emitter != convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()) ||
mxs_state.inst.size != _frontend.out_state.cfg.lengthWidth.get() ||
mxs_state.inst.maxSpeed != convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots)
mxs_state.inst.maxSpeed != (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots)
)) {
last.packet_initialize_ms = now_ms;
send_install_msg();
@ -150,7 +150,7 @@ void AP_ADSB_Sagetech_MXS::update()
last.operating_rf_select = _frontend.out_state.cfg.rfSelect;
last.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;
last.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;
last.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled;
last.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled;
last.operating_alt = _frontend._my_loc.alt;
last.packet_Operating_ms = now_ms;
@ -358,7 +358,7 @@ void AP_ADSB_Sagetech_MXS::auto_config_installation()
mxs_state.inst.sda = sg_sda_t::sdaUnknown;
mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());
mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();
mxs_state.inst.maxSpeed = convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots);
mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);
mxs_state.inst.altOffset = 0; // Alt encoder offset is legacy field that should always be 0.
mxs_state.inst.antenna = sg_antenna_t::antBottom;
@ -511,7 +511,7 @@ void AP_ADSB_Sagetech_MXS::send_install_msg()
mxs_state.inst.icao = (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get();
mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());
mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();
mxs_state.inst.maxSpeed = convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots);
mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);
mxs_state.inst.antenna = sg_antenna_t::antBottom;
last.msg.type = SG_MSG_TYPE_HOST_INSTALL;

17
libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp
View File

@ -177,22 +177,7 @@ uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char()
uint8_t encoded_null = 0;
if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) {
// not set or unknown. no bits set
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) {
encoded_null |= 0x01;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) {
encoded_null |= 0x02;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) {
encoded_null |= 0x03;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) {
encoded_null |= 0x04;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) {
encoded_null |= 0x05;
} else {
encoded_null |= 0x06;
}
encoded_null = AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);
if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) {
encoded_null |= 0x10;

4
libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp
View File

@ -411,8 +411,8 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control()
_frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request
msg.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;
msg.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;
msg.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled;
msg.es1090TxEnabled = _frontend.out_state.ctrl.es1090TxEnabled;
msg.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled;
msg.es1090TxEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.es1090TxEnabled;
// if enabled via param ADSB_OPTIONS, use squawk 7400 while in any Loss-Comms related failsafe
// https://www.faa.gov/documentLibrary/media/Notice/N_JO_7110.724_5-2-9_UAS_Lost_Link_2.pdf

28
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -346,9 +346,9 @@ void AP_AHRS::update_state(void)
state.primary_core = _get_primary_core_index();
state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec);
state.airspeed_ok = _airspeed_EAS(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_TAS(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_TAS(state.airspeed_vec);
state.quat_ok = _get_quaternion(state.quat);
state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude);
state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat);
@ -931,7 +931,7 @@ bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const
// return an airspeed estimate if available. return true
// if we have an estimate
bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
bool AP_AHRS::_airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
{
#if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED)
const uint8_t idx = get_active_airspeed_index();
@ -970,20 +970,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
airspeed_estimate_type = AirspeedEstimateType::SIM;
return sim.airspeed_estimate(airspeed_ret);
return sim.airspeed_EAS(airspeed_ret);
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#if AP_AHRS_DCM_ENABLED
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#else
return false;
#endif
@ -999,7 +999,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
case EKFType::EXTERNAL:
#if AP_AHRS_DCM_ENABLED
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#else
return false;
#endif
@ -1027,18 +1027,18 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
#if AP_AHRS_DCM_ENABLED
// fallback to DCM
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#endif
return false;
}
bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const
bool AP_AHRS::_airspeed_TAS(float &airspeed_ret) const
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm.airspeed_estimate_true(airspeed_ret);
return dcm.airspeed_TAS(airspeed_ret);
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -1064,7 +1064,7 @@ bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const
bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
@ -1126,14 +1126,14 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance,
return false;
}
// return a synthetic airspeed estimate (one derived from sensors
// return a synthetic EAS estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool AP_AHRS::synthetic_airspeed(float &ret) const
{
#if AP_AHRS_DCM_ENABLED
return dcm.synthetic_airspeed(ret);
return dcm.synthetic_airspeed_EAS(ret);
#endif
return false;
}

10
libraries/AP_AHRS/AP_AHRS.h
View File

@ -152,7 +152,7 @@ public:
// get air density / sea level density - decreases as altitude climbs
float get_air_density_ratio(void) const;
// return an airspeed estimate if available. return true
// return an (equivalent) airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const;
@ -195,7 +195,7 @@ public:
return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index);
}
// return a synthetic airspeed estimate (one derived from sensors
// return a synthetic (equivalent) airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
@ -873,7 +873,7 @@ private:
// return an airspeed estimate if available. return true
// if we have an estimate
bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const;
bool _airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &status) const;
// return secondary attitude solution if available, as eulers in radians
bool _get_secondary_attitude(Vector3f &eulers) const;
@ -892,11 +892,11 @@ private:
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool _airspeed_estimate_true(float &airspeed_ret) const;
bool _airspeed_TAS(float &airspeed_ret) const;
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool _airspeed_vector_true(Vector3f &vec) const;
bool _airspeed_TAS(Vector3f &vec) const;
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;

9
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -135,13 +135,13 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
virtual bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_estimate(airspeed_ret)) {
bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_EAS(airspeed_ret)) {
return false;
}
airspeed_ret *= get_EAS2TAS();
@ -156,6 +156,7 @@ public:
// get apparent to true airspeed ratio
static float get_EAS2TAS(void);
static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); }
// return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate

32
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -724,16 +724,16 @@ AP_AHRS_DCM::drift_correction(float deltat)
return;
}
float airspeed = _last_airspeed;
float airspeed_TAS = _last_airspeed_TAS;
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled()) {
airspeed = AP::airspeed()->get_airspeed();
airspeed_TAS = AP::airspeed()->get_airspeed();
}
#endif
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
velocity = _dcm_matrix.colx() * airspeed_TAS;
// add in wind estimate
velocity += _wind;
@ -762,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// take positive component in X direction. This mimics a pitot
// tube
_last_airspeed = MAX(airspeed.x, 0);
_last_airspeed_TAS = MAX(airspeed.x, 0);
}
if (have_gps()) {
@ -1084,31 +1084,31 @@ bool AP_AHRS_DCM::get_location(Location &loc) const
return _have_position;
}
bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
bool AP_AHRS_DCM::airspeed_EAS(float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
const auto *airspeed = AP::airspeed();
if (airspeed != nullptr) {
return airspeed_estimate(airspeed->get_primary(), airspeed_ret);
return airspeed_EAS(airspeed->get_primary(), airspeed_ret);
}
#endif
// airspeed_estimate will also make this nullptr check and act
// appropriately when we call it with a dummy sensor ID.
return airspeed_estimate(0, airspeed_ret);
return airspeed_EAS(0, airspeed_ret);
}
// return an airspeed estimate:
// return an (equivalent) airspeed estimate:
// - from a real sensor if available
// - otherwise from a GPS-derived wind-triangle estimate (if GPS available)
// - otherwise from a cached wind-triangle estimate value (but returning false)
bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
bool AP_AHRS_DCM::airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const
{
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
if (!get_unconstrained_airspeed_estimate(airspeed_index, airspeed_ret)) {
if (!get_unconstrained_airspeed_EAS(airspeed_index, airspeed_ret)) {
return false;
}
@ -1127,12 +1127,12 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
return true;
}
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled(airspeed_index)) {
@ -1143,13 +1143,13 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl
if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) {
// estimated via GPS speed and wind
airspeed_ret = _last_airspeed;
airspeed_ret = _last_airspeed_TAS;
return true;
}
// Else give the last estimate, but return false.
// This is used by the dead-reckoning code
airspeed_ret = _last_airspeed;
airspeed_ret = _last_airspeed_TAS;
return false;
}
@ -1182,7 +1182,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
Vector2f gndVelADS;
Vector2f gndVelGPS;
float airspeed = 0;
const bool gotAirspeed = airspeed_estimate_true(airspeed);
const bool gotAirspeed = airspeed_TAS(airspeed);
const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) {
const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed};

16
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -82,18 +82,18 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
bool airspeed_EAS(float &airspeed_ret) const override;
// return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override;
bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override;
// return a synthetic airspeed estimate (one derived from sensors
// return a synthetic EAS estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED {
ret = _last_airspeed;
bool synthetic_airspeed_EAS(float &ret) const WARN_IF_UNUSED {
ret = _last_airspeed_TAS;
return true;
}
@ -173,12 +173,12 @@ private:
// DCM matrix from the eulers. Called internally we may.
void reset(bool recover_eulers);
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
bool get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const;
bool get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const;
// primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix;
@ -262,7 +262,7 @@ private:
Vector3f _last_fuse;
Vector3f _last_vel;
uint32_t _last_wind_time;
float _last_airspeed;
float _last_airspeed_TAS;
uint32_t _last_consistent_heading;
// estimated wind in m/s

6
libraries/AP_AHRS/AP_AHRS_SIM.cpp
View File

@ -41,7 +41,7 @@ bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const
return true;
}
bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const
bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const
{
if (_sitl == nullptr) {
return false;
@ -52,9 +52,9 @@ bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const
return true;
}
bool AP_AHRS_SIM::airspeed_estimate(uint8_t index, float &airspeed_ret) const
bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const
{
return airspeed_estimate(airspeed_ret);
return airspeed_EAS(airspeed_ret);
}
bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const

4
libraries/AP_AHRS/AP_AHRS_SIM.h
View File

@ -68,11 +68,11 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
bool airspeed_EAS(float &airspeed_ret) const override;
// return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override;
bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override;
// return a ground vector estimate in meters/second, in North/East order
Vector2f groundspeed_vector() override;

2
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -36,7 +36,7 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
y_angle = 270;
break;
default:
AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
AP_HAL::panic("Unsupported AHRS view %u", (unsigned)rotation);
}
_pitch_trim_deg = pitch_trim_deg;

2
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
View File

@ -58,7 +58,7 @@ public:
{
AP_Param::setup_object_defaults(this, var_info);
if (_singleton != nullptr) {
AP_HAL::panic("AP_Logger must be singleton");
AP_HAL::panic("AP_AdvancedFailsafe must be singleton");
}
_singleton = this;

43
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -164,32 +164,43 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
update_calibration(i, vground, max_airspeed_allowed_during_cal);
}
#if HAL_GCS_ENABLED && AP_AIRSPEED_AUTOCAL_ENABLE
send_airspeed_calibration(vground);
#endif
}
#if HAL_GCS_ENABLED
#if HAL_GCS_ENABLED && AP_AIRSPEED_AUTOCAL_ENABLE
void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
{
#if AP_AIRSPEED_AUTOCAL_ENABLE
const mavlink_airspeed_autocal_t packet{
/*
the AIRSPEED_AUTOCAL message doesn't have an instance number
so we can only send it for one sensor at a time
*/
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (!param[i].autocal && !calibration_enabled) {
// auto-calibration not enabled on this sensor
continue;
}
const mavlink_airspeed_autocal_t packet{
vx: vground.x,
vy: vground.y,
vz: vground.z,
diff_pressure: get_differential_pressure(primary),
diff_pressure: get_differential_pressure(i),
EAS2TAS: AP::ahrs().get_EAS2TAS(),
ratio: param[primary].ratio.get(),
state_x: state[primary].calibration.state.x,
state_y: state[primary].calibration.state.y,
state_z: state[primary].calibration.state.z,
Pax: state[primary].calibration.P.a.x,
Pby: state[primary].calibration.P.b.y,
Pcz: state[primary].calibration.P.c.z
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
(const char *)&packet);
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
ratio: param[i].ratio.get(),
state_x: state[i].calibration.state.x,
state_y: state[i].calibration.state.y,
state_z: state[i].calibration.state.z,
Pax: state[i].calibration.P.a.x,
Pby: state[i].calibration.P.b.y,
Pcz: state[i].calibration.P.c.z
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
(const char *)&packet);
break; // we can only send for one sensor
}
}
#endif // HAL_GCS_ENABLED
#endif // HAL_GCS_ENABLED && AP_AIRSPEED_AUTOCAL_ENABLE
#endif // AP_AIRSPEED_ENABLED

6
libraries/AP_Arming/AP_Arming.cpp
View File

@ -1291,7 +1291,7 @@ bool AP_Arming::fence_checks(bool display_failure)
}
#endif // AP_FENCE_ENABLED
#if HAL_RUNCAM_ENABLED
#if AP_CAMERA_RUNCAM_ENABLED
bool AP_Arming::camera_checks(bool display_failure)
{
if (check_enabled(ARMING_CHECK_CAMERA)) {
@ -1309,7 +1309,7 @@ bool AP_Arming::camera_checks(bool display_failure)
}
return true;
}
#endif // HAL_RUNCAM_ENABLED
#endif // AP_CAMERA_RUNCAM_ENABLED
#if OSD_ENABLED
bool AP_Arming::osd_checks(bool display_failure) const
@ -1603,7 +1603,7 @@ bool AP_Arming::pre_arm_checks(bool report)
#if HAL_PROXIMITY_ENABLED
& proximity_checks(report)
#endif
#if HAL_RUNCAM_ENABLED
#if AP_CAMERA_RUNCAM_ENABLED
& camera_checks(report)
#endif
#if OSD_ENABLED

96
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -69,6 +69,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: _
// @Path: AP_BattMonitor_ESC.cpp
// @Group: _
// @Path: AP_BattMonitor_INA239.cpp
// @Group: _
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: _
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[0], "_", 41, AP_BattMonitor, backend_var_info[0]),
#if AP_BATT_MONITOR_MAX_INSTANCES > 1
@ -92,6 +98,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 2_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 2_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 2_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 2_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]),
#endif
@ -116,6 +128,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 3_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 3_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 3_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 3_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]),
#endif
@ -140,6 +158,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 4_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 4_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 4_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 4_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]),
#endif
@ -164,6 +188,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 5_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 5_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 5_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 5_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]),
#endif
@ -188,6 +218,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 6_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 6_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 6_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 6_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]),
#endif
@ -212,6 +248,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 7_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 7_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 7_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 7_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]),
#endif
@ -236,6 +278,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 8_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 8_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 8_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 8_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]),
#endif
@ -260,6 +308,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: 9_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: 9_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: 9_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: 9_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]),
#endif
@ -284,6 +338,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: A_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: A_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: A_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: A_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[9], "A_", 50, AP_BattMonitor, backend_var_info[9]),
#endif
@ -308,6 +368,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: B_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: B_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: B_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: B_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[10], "B_", 51, AP_BattMonitor, backend_var_info[10]),
#endif
@ -332,6 +398,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: C_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: C_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: C_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: C_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[11], "C_", 52, AP_BattMonitor, backend_var_info[11]),
#endif
@ -356,6 +428,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: D_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: D_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: D_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: D_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[12], "D_", 53, AP_BattMonitor, backend_var_info[12]),
#endif
@ -380,6 +458,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: E_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: E_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: E_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: E_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[13], "E_", 54, AP_BattMonitor, backend_var_info[13]),
#endif
@ -404,6 +488,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: F_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: F_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: F_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: F_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[14], "F_", 55, AP_BattMonitor, backend_var_info[14]),
#endif
@ -428,6 +518,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
// @Path: AP_BattMonitor_INA2xx.cpp
// @Group: G_
// @Path: AP_BattMonitor_ESC.cpp
// @Group: G_
// @Path: AP_BattMonitor_INA239.cpp
// @Group: G_
// @Path: AP_BattMonitor_INA3221.cpp
// @Group: G_
// @Path: AP_BattMonitor_AD7091R5.cpp
AP_SUBGROUPVARPTR(drivers[15], "G_", 56, AP_BattMonitor, backend_var_info[15]),
#endif

2
libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp
View File

@ -84,7 +84,7 @@ const AP_Param::GroupInfo AP_BattMonitor_AD7091R5::var_info[] = {
// @User: Advanced
AP_GROUPINFO("VLT_OFFSET", 61, AP_BattMonitor_AD7091R5, _volt_offset, 0),
// Param indexes must be 56 to 61 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

2
libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp
View File

@ -55,7 +55,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Analog::var_info[] = {
// @User: Advanced
AP_GROUPINFO("VLT_OFFSET", 6, AP_BattMonitor_Analog, _volt_offset, 0),
// Param indexes must be less than 10 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

21
libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
View File

@ -26,6 +26,27 @@
#include "AP_ESC_Telem/AP_ESC_Telem.h"
#endif
/*
All backends use the same parameter table and set of indices. Therefore, two
backends must not use the same index. The list of used indices and
corresponding backends is below.
1-6: AP_BattMonitor_Analog.cpp
10-11: AP_BattMonitor_SMBus.cpp
20: AP_BattMonitor_Sum.cpp
22-24: AP_BattMonitor_INA3221.cpp
25-26: AP_BattMonitor_INA2xx.cpp
27-28: AP_BattMonitor_INA2xx.cpp, AP_BattMonitor_INA239.cpp (legacy duplication)
30: AP_BattMonitor_DroneCAN.cpp
36: AP_BattMonitor_ESC.cpp
40-43: AP_BattMonitor_FuelLevel_Analog.cpp
45-48: AP_BattMonitor_FuelLevel_Analog.cpp
50-51: AP_BattMonitor_Synthetic_Current.cpp
56-61: AP_BattMonitor_AD7091R5.cpp
Usage does not need to be contiguous. The maximum possible index is 63.
*/
/*
base class constructor.
This incorporates initialisation as well.

2
libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp
View File

@ -26,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_DroneCAN::var_info[] = {
// @User: Advanced
AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_DroneCAN, _curr_mult, 1.0),
// Param indexes must be between 30 and 35 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

4
libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp
View File

@ -22,8 +22,6 @@
const AP_Param::GroupInfo AP_BattMonitor_ESC::var_info[] = {
// Param indexes must be between 36 and 39 to avoid conflict with other battery monitor param tables loaded by pointer
// @Param: ESC_MASK
// @DisplayName: ESC mask
// @Description: If 0 all connected ESCs will be used. If non-zero, only those selected in will be used.
@ -31,7 +29,7 @@ const AP_Param::GroupInfo AP_BattMonitor_ESC::var_info[] = {
// @User: Standard
AP_GROUPINFO("ESC_MASK", 36, AP_BattMonitor_ESC, _mask, 0),
// Param indexes must be between 36 and 39 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

4
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp
View File

@ -58,8 +58,6 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = {
// @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 5:Navigator, 13:Pixhawk2_PM2/CubeOrange_PM2, 14:CubeOrange, 16:Durandal, 100:PX4-v1
AP_GROUPINFO("FL_PIN", 43, AP_BattMonitor_FuelLevel_Analog, _pin, -1),
// index 44 unused and available
// @Param: FL_FF
// @DisplayName: First order term
// @Description: First order polynomial fit term
@ -88,7 +86,7 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = {
// @User: Advanced
AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0),
// Param indexes must be between 40 and 49 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

4
libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp
View File

@ -47,7 +47,9 @@ const AP_Param::GroupInfo AP_BattMonitor_INA239::var_info[] = {
// @Units: Ohm
// @User: Advanced
AP_GROUPINFO("SHUNT", 28, AP_BattMonitor_INA239, rShunt, HAL_BATTMON_INA239_SHUNT_RESISTANCE),
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

4
libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
View File

@ -108,7 +108,9 @@ const AP_Param::GroupInfo AP_BattMonitor_INA2XX::var_info[] = {
// @Units: Ohm
// @User: Advanced
AP_GROUPINFO("SHUNT", 28, AP_BattMonitor_INA2XX, rShunt, DEFAULT_BATTMON_INA2XX_SHUNT),
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

10
libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp
View File

@ -67,15 +67,13 @@ uint8_t AP_BattMonitor_INA3221::address_driver_count;
const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {
// Param indexes must be between 56 and 61 to avoid conflict with other battery monitor param tables loaded by pointer
// @Param: I2C_BUS
// @DisplayName: Battery monitor I2C bus number
// @Description: Battery monitor I2C bus number
// @Range: 0 3
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("I2C_BUS", 56, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS),
AP_GROUPINFO("I2C_BUS", 22, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS),
// @Param: I2C_ADDR
// @DisplayName: Battery monitor I2C address
@ -83,7 +81,7 @@ const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {
// @Range: 0 127
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("I2C_ADDR", 57, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR),
AP_GROUPINFO("I2C_ADDR", 23, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR),
// @Param: CHANNEL
// @DisplayName: INA3221 channel
@ -91,7 +89,9 @@ const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {
// @Range: 1 3
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("CHANNEL", 58, AP_BattMonitor_INA3221, channel, 1),
AP_GROUPINFO("CHANNEL", 24, AP_BattMonitor_INA3221, channel, 1),
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

4
libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
View File

@ -113,7 +113,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
// @DisplayName: Low battery failsafe action
// @Description: What action the vehicle should perform if it hits a low battery failsafe
// @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,6:Loiter to QLand
// @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START or RTL
// @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL
// @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode
// @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate
// @Values{Tracker}: 0:None
@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
// @DisplayName: Critical battery failsafe action
// @Description: What action the vehicle should perform if it hits a critical battery failsafe
// @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,5:Parachute,6:Loiter to QLand
// @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START or RTL
// @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL
// @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode
// @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate
// @Values{Tracker}: 0:None

4
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp
View File

@ -10,8 +10,6 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_BattMonitor_SMBus::var_info[] = {
// Param indexes must be between 10 and 19 to avoid conflict with other battery monitor param tables loaded by pointer
// @Param: I2C_BUS
// @DisplayName: Battery monitor I2C bus number
// @Description: Battery monitor I2C bus number
@ -28,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_SMBus::var_info[] = {
// @RebootRequired: True
AP_GROUPINFO("I2C_ADDR", 11, AP_BattMonitor_SMBus, _address, AP_BATTMONITOR_SMBUS_I2C_ADDR),
// Param indexes must be between 10 and 19 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

4
libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp
View File

@ -19,8 +19,6 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_BattMonitor_Sum::var_info[] = {
// Param indexes must be between 20 and 29 to avoid conflict with other battery monitor param tables loaded by pointer
// @Param: SUM_MASK
// @DisplayName: Battery Sum mask
// @Description: 0: sum of remaining battery monitors, If none 0 sum of specified monitors. Current will be summed and voltages averaged.
@ -28,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Sum::var_info[] = {
// @User: Standard
AP_GROUPINFO("SUM_MASK", 20, AP_BattMonitor_Sum, _sum_mask, 0),
// Param indexes must be between 20 and 29 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

2
libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp
View File

@ -27,7 +27,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Synthetic_Current::var_info[] = {
// also inherit analog backend parameters
AP_SUBGROUPEXTENSION("", 51, AP_BattMonitor_Synthetic_Current, AP_BattMonitor_Analog::var_info),
// Param indexes must be between 50 and 55 to avoid conflict with other battery monitor param tables loaded by pointer
// CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
AP_GROUPEND
};

8
libraries/AP_CANManager/AP_CANIfaceParams.cpp
View File

@ -43,6 +43,14 @@ const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = {
// @User: Advanced
AP_GROUPINFO("FDBITRATE", 3, AP_CANManager::CANIface_Params, _fdbitrate, HAL_CANFD_SUPPORTED),
#endif
// @Param: OPTIONS
// @DisplayName: CAN per-interface options
// @Description: CAN per-interface options
// @Bitmask: 0:LogAllFrames
// @User: Advanced
AP_GROUPINFO("OPTIONS", 4, AP_CANManager::CANIface_Params, _options, 0),
// Index 3 occupied by Param: DEBUG
AP_GROUPEND
};

76
libraries/AP_CANManager/AP_CANManager.cpp
View File

@ -41,6 +41,15 @@
#include <AP_Common/ExpandingString.h>
#include <AP_Common/sorting.h>
#include <AP_Logger/AP_Logger.h>
/*
avoid a recursion issue with config defines
*/
#if AP_CAN_LOGGING_ENABLED && !HAL_LOGGING_ENABLED
#undef AP_CAN_LOGGING_ENABLED
#define AP_CAN_LOGGING_ENABLED 0
#endif
#define LOG_TAG "CANMGR"
#define LOG_BUFFER_SIZE 1024
@ -260,6 +269,10 @@ void AP_CANManager::init()
_drivers[drv_num]->init(drv_num, enable_filter);
}
#if AP_CAN_LOGGING_ENABLED
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::check_logging_enable, void));
#endif
}
#else
void AP_CANManager::init()
@ -372,6 +385,7 @@ void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag,
if (loglevel > _loglevel) {
return;
}
WITH_SEMAPHORE(_sem);
if ((LOG_BUFFER_SIZE - _log_pos) < (10 + strlen(tag) + strlen(fmt))) {
// reset log pos
@ -509,6 +523,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg)
frame_buffer->push(frame);
break;
}
#if HAL_CANFD_SUPPORTED
case MAVLINK_MSG_ID_CANFD_FRAME: {
mavlink_canfd_frame_t p;
mavlink_msg_canfd_frame_decode(&msg, &p);
@ -523,6 +538,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg)
frame_buffer->push(frame);
break;
}
#endif
}
process_frame_buffer();
}
@ -684,12 +700,15 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
}
}
const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
#if HAL_CANFD_SUPPORTED
if (frame.isCanFDFrame()) {
if (HAVE_PAYLOAD_SPACE(can_forward.chan, CANFD_FRAME)) {
mavlink_msg_canfd_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
}
} else {
} else
#endif
{
if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) {
mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
@ -698,6 +717,61 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
}
#endif // HAL_GCS_ENABLED
#if AP_CAN_LOGGING_ENABLED
/*
handler for CAN frames for frame logging
*/
void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
{
#if HAL_CANFD_SUPPORTED
if (frame.canfd) {
struct log_CAFD pkt {
LOG_PACKET_HEADER_INIT(LOG_CAFD_MSG),
time_us : AP_HAL::micros64(),
bus : bus,
id : frame.id,
dlc : frame.dlc
};
memcpy(pkt.data, frame.data, frame.dlcToDataLength(frame.dlc));
AP::logger().WriteBlock(&pkt, sizeof(pkt));
return;
}
#endif
struct log_CANF pkt {
LOG_PACKET_HEADER_INIT(LOG_CANF_MSG),
time_us : AP_HAL::micros64(),
bus : bus,
id : frame.id,
dlc : frame.dlc
};
memcpy(pkt.data, frame.data, frame.dlc);
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
/*
see if we need to enable/disable the CAN logging callback
*/
void AP_CANManager::check_logging_enable(void)
{
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
const bool enabled = _interfaces[i].option_is_set(CANIface_Params::Options::LOG_ALL_FRAMES);
uint8_t &logging_id = _interfaces[i].logging_id;
auto *can = hal.can[i];
if (can == nullptr) {
continue;
}
if (enabled && logging_id == 0) {
can->register_frame_callback(
FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &),
logging_id);
} else if (!enabled && logging_id != 0) {
can->unregister_frame_callback(logging_id);
}
}
}
#endif // AP_CAN_LOGGING_ENABLED
AP_CANManager& AP::can()
{
return *AP_CANManager::get_singleton();

21
libraries/AP_CANManager/AP_CANManager.h
View File

@ -126,10 +126,23 @@ private:
static const struct AP_Param::GroupInfo var_info[];
enum class Options : uint32_t {
LOG_ALL_FRAMES = (1U<<0),
};
bool option_is_set(Options option) const {
return (_options & uint32_t(option)) != 0;
}
private:
AP_Int8 _driver_number;
AP_Int32 _bitrate;
AP_Int32 _fdbitrate;
AP_Int32 _options;
#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED
uint8_t logging_id;
#endif
};
//Parameter Interface for CANDrivers
@ -198,6 +211,14 @@ private:
void process_frame_buffer(void);
#endif // HAL_GCS_ENABLED
#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED
/*
handler for CAN frames for logging
*/
void can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame);
void check_logging_enable(void);
#endif
};
namespace AP

5
libraries/AP_CANManager/AP_CANManager_config.h
View File

@ -5,3 +5,8 @@
#ifndef AP_CAN_SLCAN_ENABLED
#define AP_CAN_SLCAN_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS
#endif
#ifndef AP_CAN_LOGGING_ENABLED
#define AP_CAN_LOGGING_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS
#endif

79
libraries/AP_CANManager/LogStructure.h Normal file
View File

@ -0,0 +1,79 @@
#pragma once
/*
log structures for AP_CANManager
*/
#include <AP_Logger/LogStructure.h>
#include "AP_CANManager_config.h"
#define LOG_IDS_FROM_CANMANAGER \
LOG_CANF_MSG, \
LOG_CAFD_MSG
// @LoggerMessage: CANF
// @Description: CAN Frame
// @Field: TimeUS: Time since system startup
// @Field: Bus: bus number
// @Field: Id: frame identifier
// @Field: DLC: data length code
// @Field: B0: byte 0
// @Field: B1: byte 1
// @Field: B2: byte 2
// @Field: B3: byte 3
// @Field: B4: byte 4
// @Field: B5: byte 5
// @Field: B6: byte 6
// @Field: B7: byte 7
struct PACKED log_CANF {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t bus;
uint32_t id;
uint8_t dlc;
uint8_t data[8];
};
// @LoggerMessage: CAFD
// @Description: CANFD Frame
// @Field: TimeUS: Time since system startup
// @Field: Bus: bus number
// @Field: Id: frame identifier
// @Field: DLC: data length code
// @Field: D0: data 0
// @Field: D1: data 1
// @Field: D2: data 2
// @Field: D3: data 3
// @Field: D4: data 4
// @Field: D5: data 5
// @Field: D6: data 6
// @Field: D7: data 7
struct PACKED log_CAFD {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t bus;
uint32_t id;
uint8_t dlc;
uint64_t data[8];
};
#if !AP_CAN_LOGGING_ENABLED
#define LOG_STRUCTURE_FROM_CANMANAGER
#else
#define LOG_STRUCTURE_FROM_CANMANAGER \
{ LOG_CANF_MSG, sizeof(log_CANF), \
"CANF", \
"Q" "B" "I" "B" "B" "B" "B" "B" "B" "B" "B" "B", \
"TimeUS," "Bus," "Id," "DLC," "B0," "B1," "B2," "B3," "B4," "B5," "B6," "B7", \
"s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \
"F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \
false \
}, \
{ LOG_CAFD_MSG, sizeof(log_CAFD), \
"CAFD", \
"Q" "B" "I" "B" "Q" "Q" "Q" "Q" "Q" "Q" "Q" "Q", \
"TimeUS," "Bus," "Id," "DLC," "D0," "D1," "D2," "D3," "D4," "D5," "D6," "D7", \
"s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \
"F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \
false \
},
#endif // AP_CAN_LOGGING_ENABLED

69
libraries/AP_Camera/AP_Camera.cpp
View File

@ -6,6 +6,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include "AP_Camera_Backend.h"
#include "AP_Camera_Servo.h"
#include "AP_Camera_Relay.h"
@ -14,6 +15,7 @@
#include "AP_Camera_MAVLink.h"
#include "AP_Camera_MAVLinkCamV2.h"
#include "AP_Camera_Scripting.h"
#include "AP_RunCam.h"
const AP_Param::GroupInfo AP_Camera::var_info[] = {
@ -41,10 +43,24 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = {
// @Path: AP_Camera_Params.cpp
AP_SUBGROUPINFO(_params[1], "2", 13, AP_Camera, AP_Camera_Params),
#endif
#if AP_CAMERA_RUNCAM_ENABLED
// @Group: 1_RC_
// @Path: AP_RunCam.cpp
AP_SUBGROUPVARPTR(_backends[0], "1_RC_", 14, AP_Camera, _backend_var_info[0]),
#if AP_CAMERA_MAX_INSTANCES > 1
// @Group: 2_RC_
// @Path: AP_RunCam.cpp
AP_SUBGROUPVARPTR(_backends[1], "2_RC_", 15, AP_Camera, _backend_var_info[1]),
#endif
#endif
AP_GROUPEND
};
#if AP_CAMERA_RUNCAM_ENABLED
const AP_Param::GroupInfo *AP_Camera::_backend_var_info[AP_CAMERA_MAX_INSTANCES];
#endif
extern const AP_HAL::HAL& hal;
AP_Camera::AP_Camera(uint32_t _log_camera_bit) :
@ -238,6 +254,17 @@ void AP_Camera::init()
case CameraType::SCRIPTING:
_backends[instance] = NEW_NOTHROW AP_Camera_Scripting(*this, _params[instance], instance);
break;
#endif
#if AP_CAMERA_RUNCAM_ENABLED
// check for RunCam driver
case CameraType::RUNCAM:
if (_backends[instance] == nullptr) { // may have already been created by the conversion code
_backends[instance] = NEW_NOTHROW AP_RunCam(*this, _params[instance], instance, _runcam_instances);
_backend_var_info[instance] = AP_RunCam::var_info;
AP_Param::load_object_from_eeprom(_backends[instance], _backend_var_info[instance]);
_runcam_instances++;
}
break;
#endif
case CameraType::NONE:
break;
@ -899,7 +926,11 @@ AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const
void AP_Camera::convert_params()
{
// exit immediately if CAM1_TYPE has already been configured
if (_params[0].type.configured()) {
if (_params[0].type.configured()
#if AP_CAMERA_RUNCAM_ENABLED
&& _params[1].type.configured()
#endif
) {
return;
}
@ -919,6 +950,42 @@ void AP_Camera::convert_params()
}
_params[0].type.set_and_save(cam1_type);
#if AP_CAMERA_RUNCAM_ENABLED
// RunCam PARAMETER_CONVERSION - Added: Nov-2024 ahead of 4.7 release
// Since slot 1 is essentially used by the trigger type, we will use slot 2 for runcam
int8_t rc_type = 0;
// find vehicle's top level key
uint16_t k_param_vehicle_key;
if (!AP_Param::find_top_level_key_by_pointer(AP::vehicle(), k_param_vehicle_key)) {
return;
}
// RunCam protocol configured so set cam type to RunCam
bool rc_protocol_configured = false;
AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
if (serial_manager && serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0)) {
rc_protocol_configured = true;
}
const AP_Param::ConversionInfo rc_type_info = {
k_param_vehicle_key, AP_GROUP_ELEM_IDX(1, 1), AP_PARAM_INT8, "CAM_RC_TYPE"
};
AP_Int8 rc_type_old;
const bool found_rc_type = AP_Param::find_old_parameter(&rc_type_info, &rc_type_old);
if (rc_protocol_configured || (found_rc_type && rc_type_old.get() > 0)) {
rc_type = int8_t(CameraType::RUNCAM);
_backends[1] = NEW_NOTHROW AP_RunCam(*this, _params[1], 1, _runcam_instances);
_backend_var_info[1] = AP_RunCam::var_info;
AP_Param::convert_class(k_param_vehicle_key, &_backends[1], _backend_var_info[1], 1, false);
AP_Param::invalidate_count();
_runcam_instances++;
}
_params[1].type.set_and_save(rc_type);
#endif // AP_CAMERA_RUNCAM_ENABLED
// convert CAM_DURATION (in deci-seconds) to CAM1_DURATION (in seconds)
int8_t cam_duration = 0;
if (AP_Param::get_param_by_index(this, 1, AP_PARAM_INT8, &cam_duration) && (cam_duration > 0)) {

10
libraries/AP_Camera/AP_Camera.h
View File

@ -23,6 +23,7 @@ class AP_Camera_Mount;
class AP_Camera_MAVLink;
class AP_Camera_MAVLinkCamV2;
class AP_Camera_Scripting;
class AP_RunCam;
/// @class Camera
/// @brief Object managing a Photo or video camera
@ -37,6 +38,7 @@ class AP_Camera {
friend class AP_Camera_MAVLink;
friend class AP_Camera_MAVLinkCamV2;
friend class AP_Camera_Scripting;
friend class AP_RunCam;
public:
@ -72,6 +74,9 @@ public:
#endif
#if AP_CAMERA_SCRIPTING_ENABLED
SCRIPTING = 7, // Scripting backend
#endif
#if AP_CAMERA_RUNCAM_ENABLED
RUNCAM = 8, // RunCam backend
#endif
};
@ -216,6 +221,11 @@ protected:
// parameters for backends
AP_Camera_Params _params[AP_CAMERA_MAX_INSTANCES];
#if AP_CAMERA_RUNCAM_ENABLED
// var info pointer for RunCam
static const struct AP_Param::GroupInfo *_backend_var_info[AP_CAMERA_MAX_INSTANCES];
uint8_t _runcam_instances;
#endif
private:

43
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -274,7 +274,6 @@ void AP_Camera_Backend::send_video_stream_information(mavlink_channel_t chan) co
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
void AP_Camera_Backend::set_stream_information(mavlink_video_stream_information_t stream_info)
{
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera %u VIDEO_STREAM_INFORMATION (%s) set from script", _instance, stream_info.name);
_stream_info = stream_info;
};
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
@ -296,32 +295,46 @@ void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
{
// getting corresponding mount instance for camera
const AP_Mount* mount = AP::mount();
AP_Mount* mount = AP::mount();
if (mount == nullptr) {
return;
}
// get latest POI from mount
Quaternion quat;
Location loc;
Location camera_loc;
Location poi_loc;
if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) {
return;
const bool have_poi_loc = mount->get_poi(get_mount_instance(), quat, camera_loc, poi_loc);
// if failed to get POI, get camera location directly from AHRS
// and attitude directly from mount
bool have_camera_loc = have_poi_loc;
if (!have_camera_loc) {
have_camera_loc = AP::ahrs().get_location(camera_loc);
mount->get_attitude_quaternion(get_mount_instance(), quat);
}
// calculate attitude quaternion in earth frame using AHRS yaw
Quaternion quat_ef;
quat_ef.from_euler(0, 0, AP::ahrs().get_yaw());
quat_ef *= quat;
// send camera fov status message only if the last calculated values aren't stale
const float quat_array[4] = {
quat.q1,
quat.q2,
quat.q3,
quat.q4
quat_ef.q1,
quat_ef.q2,
quat_ef.q3,
quat_ef.q4
};
mavlink_msg_camera_fov_status_send(
chan,
AP_HAL::millis(),
loc.lat,
loc.lng,
loc.alt * 10,
poi_loc.lat,
poi_loc.lng,
poi_loc.alt * 10,
have_camera_loc ? camera_loc.lat : INT32_MAX,
have_camera_loc ? camera_loc.lng : INT32_MAX,
have_camera_loc ? camera_loc.alt * 10 : INT32_MAX,
have_poi_loc ? poi_loc.lat : INT32_MAX,
have_poi_loc ? poi_loc.lng : INT32_MAX,
have_poi_loc ? poi_loc.alt * 10 : INT32_MAX,
quat_array,
horizontal_fov() > 0 ? horizontal_fov() : NaNf,
vertical_fov() > 0 ? vertical_fov() : NaNf

2
libraries/AP_Camera/AP_Camera_Params.cpp
View File

@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
// @Param: _TYPE
// @DisplayName: Camera shutter (trigger) type
// @Description: how to trigger the camera to take a picture
// @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting
// @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting, 8:RunCam
// @User: Standard
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE),

4
libraries/AP_Camera/AP_Camera_config.h
View File

@ -62,3 +62,7 @@
#ifndef AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
#define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED AP_CAMERA_SCRIPTING_ENABLED
#endif
#ifndef AP_CAMERA_RUNCAM_ENABLED
#define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && HAL_RUNCAM_ENABLED
#endif

129
libraries/AP_Camera/AP_RunCam.cpp
View File

@ -23,7 +23,7 @@
*/
#include "AP_RunCam.h"
#if HAL_RUNCAM_ENABLED
#if AP_CAMERA_RUNCAM_ENABLED
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
@ -36,7 +36,7 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = {
// @DisplayName: RunCam device type
// @Description: RunCam device type used to determine OSD menu structure and shutter options.
// @Values: 0:Disabled, 1:RunCam Split Micro/RunCam with UART, 2:RunCam Split, 3:RunCam Split4 4k, 4:RunCam Hybrid/RunCam Thumb Pro, 5:Runcam 2 4k
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceType::Disabled), AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceModel::SplitMicro), AP_PARAM_FLAG_ENABLE),
// @Param: FEATURES
// @DisplayName: RunCam features available
@ -55,13 +55,13 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = {
// @DisplayName: RunCam button delay before allowing further button presses
// @Description: Time it takes for the a RunCam button press to be actived in ms. If this is too short then commands can get out of sync.
// @User: Advanced
AP_GROUPINFO("BTN_DELAY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY),
AP_GROUPINFO("BTN_DELY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY),
// @Param: MDE_DELAY
// @DisplayName: RunCam mode delay before allowing further button presses
// @Description: Time it takes for the a RunCam mode button press to be actived in ms. If a mode change first requires a video recording change then double this value is used. If this is too short then commands can get out of sync.
// @User: Advanced
AP_GROUPINFO("MDE_DELAY", 5, AP_RunCam, _mode_delay_ms, 800),
AP_GROUPINFO("MDE_DELY", 5, AP_RunCam, _mode_delay_ms, 800),
// @Param: CONTROL
// @DisplayName: RunCam control option
@ -118,13 +118,24 @@ AP_RunCam::Menu AP_RunCam::_menus[RUNCAM_MAX_DEVICE_TYPES] = {
{ 6, { 3, 10, 2, 2, 8 }}, // Runcam 2 4K
};
AP_RunCam::AP_RunCam()
const char* AP_RunCam::_models[RUNCAM_MAX_DEVICE_TYPES] = {
"SplitMicro",
"Split",
"Split4k",
"Hybrid",
"Run24k"
};
AP_RunCam::AP_RunCam(AP_Camera &frontend, AP_Camera_Params &params, uint8_t instance, uint8_t runcam_instance)
: AP_Camera_Backend(frontend, params, instance), _runcam_instance(runcam_instance)
{
AP_Param::setup_object_defaults(this, var_info);
if (_singleton != nullptr) {
AP_HAL::panic("AP_RunCam must be singleton");
if (_singleton != nullptr && _singleton->_instance == instance) {
AP_HAL::panic("AP_RunCam instance must be a singleton %u", instance);
}
if (_singleton == nullptr) {
_singleton = this;
}
_singleton = this;
_cam_type.set(constrain_int16(_cam_type, 0, RUNCAM_MAX_DEVICE_TYPES));
_video_recording = VideoOption(_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT));
}
@ -134,19 +145,19 @@ void AP_RunCam::init()
{
AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
if (serial_manager) {
uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0);
uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, _runcam_instance);
}
if (uart != nullptr) {
/*
if the user has setup a serial port as a runcam then default
type to the split micro (Andy's development platform!). This makes setup a bit easier for most
users while still enabling parameters to be hidden for users
without a runcam
without a RunCam
*/
_cam_type.set_default(int8_t(DeviceType::SplitMicro));
_cam_type.set_default(int8_t(DeviceModel::SplitMicro));
AP_Param::invalidate_count();
}
if (_cam_type.get() == int8_t(DeviceType::Disabled)) {
if (_cam_type.get() == int8_t(DeviceModel::Disabled)) {
uart = nullptr;
return;
}
@ -156,7 +167,7 @@ void AP_RunCam::init()
}
// Split and Runcam 2 4k requires two mode presses to get into the menu
if (_cam_type.get() == int8_t(DeviceType::Split) || _cam_type.get() == int8_t(DeviceType::Run24k)) {
if (_cam_type.get() == int8_t(DeviceModel::Split) || _cam_type.get() == int8_t(DeviceModel::Run24k)) {
_menu_enter_level = -1;
_in_menu = -1;
}
@ -221,7 +232,7 @@ void AP_RunCam::osd_option() {
// input update loop
void AP_RunCam::update()
{
if (uart == nullptr || _cam_type.get() == int8_t(DeviceType::Disabled)) {
if (uart == nullptr || _cam_type.get() == int8_t(DeviceModel::Disabled)) {
return;
}
@ -551,12 +562,12 @@ void AP_RunCam::handle_2_key_simulation_process(Event ev)
case Event::IN_MENU_ENTER:
// in a sub-menu and save-and-exit was selected
if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) {
if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) {
simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _button_delay_ms);
_sub_menu_pos = 0;
_in_menu--;
// in the top-menu and save-and-exit was selected
} else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) {
} else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) {
simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _mode_delay_ms);
_in_menu--;
_state = State::EXITING_MENU;
@ -712,7 +723,7 @@ void AP_RunCam::handle_5_key_simulation_response(const Request& request)
// command to start recording
AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const {
if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) {
if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) {
return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN;
} else {
return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING;
@ -721,7 +732,7 @@ AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const {
// command to stop recording
AP_RunCam::ControlOperation AP_RunCam::stop_recording_command() const {
if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) {
if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) {
return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN;
} else {
return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING;
@ -1058,8 +1069,88 @@ uint8_t AP_RunCam::Request::get_expected_response_length(const Command command)
return 0;
}
// AP_Camera API
// return true if healthy
bool AP_RunCam::healthy() const
{
return camera_ready();
}
// momentary switch to change camera between picture and video modes
void AP_RunCam::cam_mode_toggle()
{
}
// entry point to actually take a picture. returns true on success
bool AP_RunCam::trigger_pic()
{
return false;
}
// send camera information message to GCS
void AP_RunCam::send_camera_information(mavlink_channel_t chan) const
{
// exit immediately if not initialised
if (!camera_ready() || _cam_type.get() <= 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) {
return;
}
static const uint8_t vendor_name[32] = "RunCam";
uint8_t model_name[32] {};
strncpy((char *)model_name, _models[_cam_type.get()-1], MIN(sizeof(model_name), sizeof(_models[_cam_type.get()-1])));
const char cam_definition_uri[140] {};
// capability flags
uint32_t flags = 0;
if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) {
flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO;
}
if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) {
flags |= CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
}
// send CAMERA_INFORMATION message
mavlink_msg_camera_information_send(
chan,
AP_HAL::millis(), // time_boot_ms
vendor_name, // vendor_name uint8_t[32]
model_name, // model_name uint8_t[32]
0, // firmware version uint32_t
NaNf, // focal_length float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id uint8_t
flags, // flags uint32_t (CAMERA_CAP_FLAGS)
0, // cam_definition_version uint16_t
cam_definition_uri, // cam_definition_uri char[140]
_instance + 1); // gimbal_device_id uint8_t
}
// send camera settings message to GCS
void AP_RunCam::send_camera_settings(mavlink_channel_t chan) const
{
// exit immediately if not initialised
if (!camera_ready()) {
return;
}
// send CAMERA_SETTINGS message
mavlink_msg_camera_settings_send(
chan,
AP_HAL::millis(), // time_boot_ms
_video_recording == VideoOption::RECORDING ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
AP_RunCam *AP::runcam() {
return AP_RunCam::get_singleton();
}
#endif // HAL_RUNCAM_ENABLED
#endif // AP_CAMERA_RUNCAM_ENABLED

49
libraries/AP_Camera/AP_RunCam.h
View File

@ -23,7 +23,9 @@
#include "AP_Camera_config.h"
#if HAL_RUNCAM_ENABLED
#include "AP_Camera_Backend.h"
#if AP_CAMERA_RUNCAM_ENABLED
#include <AP_Param/AP_Param.h>
#include <RC_Channel/RC_Channel.h>
@ -38,10 +40,10 @@
/// @class AP_RunCam
/// @brief Object managing a RunCam device
class AP_RunCam
class AP_RunCam : public AP_Camera_Backend
{
public:
AP_RunCam();
AP_RunCam(AP_Camera &frontend, AP_Camera_Params &params, uint8_t instance, uint8_t runcam_instance);
// do not allow copies
CLASS_NO_COPY(AP_RunCam);
@ -51,7 +53,7 @@ public:
return _singleton;
}
enum class DeviceType {
enum class DeviceModel {
Disabled = 0,
SplitMicro = 1, // video support only
Split = 2, // camera and video support
@ -79,22 +81,49 @@ public:
VIDEO_RECORDING_AT_BOOT = (1 << 4)
};
// return true if healthy
bool healthy() const override;
// momentary switch to change camera between picture and video modes
void cam_mode_toggle() override;
// entry point to actually take a picture. returns true on success
bool trigger_pic() override;
// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan) const override;
// send camera settings message to GCS
void send_camera_settings(mavlink_channel_t chan) const override;
// initialize the RunCam driver
void init();
void init() override;
// camera button simulation
bool simulate_camera_button(const ControlOperation operation, const uint32_t transition_timeout = RUNCAM_DEFAULT_BUTTON_PRESS_DELAY);
// start the video
void start_recording();
// stop the video
void stop_recording();
// start or stop video recording. returns true on success
// set start_recording = true to start record, false to stop recording
bool record_video(bool _start_recording) override {
if (_start_recording) {
start_recording();
} else {
stop_recording();
}
return true;
}
// enter the OSD menu
void enter_osd();
// exit the OSD menu
void exit_osd();
// OSD control determined by camera options
void osd_option();
// update loop
void update();
// update - should be called at 50hz
void update() override;
// Check whether arming is allowed
bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const;
@ -265,6 +294,10 @@ private:
static uint8_t _sub_menu_lengths[RUNCAM_NUM_SUB_MENUS];
// shared inbound scratch space
uint8_t _recv_buf[RUNCAM_MAX_PACKET_SIZE]; // all the response contexts use same recv buffer
// the runcam instance
uint8_t _runcam_instance;
static const char* _models[RUNCAM_MAX_DEVICE_TYPES];
class Request;
@ -435,4 +468,4 @@ namespace AP
AP_RunCam *runcam();
};
#endif // HAL_RUNCAM_ENABLED
#endif // AP_CAMERA_RUNCAM_ENABLED

26
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -1452,7 +1452,7 @@ void AP_DDS_Client::write_time_topic()
const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: XRCE_Client failed to serialize\n");
// AP_HAL::panic("FATAL: XRCE_Client failed to serialize");
}
}
}
@ -1468,7 +1468,7 @@ void AP_DDS_Client::write_nav_sat_fix_topic()
const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1485,7 +1485,7 @@ void AP_DDS_Client::write_static_transforms()
const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1502,7 +1502,7 @@ void AP_DDS_Client::write_battery_state_topic()
const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1519,7 +1519,7 @@ void AP_DDS_Client::write_local_pose_topic()
const bool success = geometry_msgs_msg_PoseStamped_serialize_topic(&ub, &local_pose_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1536,7 +1536,7 @@ void AP_DDS_Client::write_tx_local_velocity_topic()
const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &tx_local_velocity_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1552,7 +1552,7 @@ void AP_DDS_Client::write_tx_local_airspeed_topic()
const bool success = geometry_msgs_msg_Vector3Stamped_serialize_topic(&ub, &tx_local_airspeed_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1568,7 +1568,7 @@ void AP_DDS_Client::write_imu_topic()
const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1585,7 +1585,7 @@ void AP_DDS_Client::write_geo_pose_topic()
const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_pose_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1602,7 +1602,7 @@ void AP_DDS_Client::write_clock_topic()
const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1618,7 +1618,7 @@ void AP_DDS_Client::write_gps_global_origin_topic()
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB)].dw_id, &ub, topic_size);
const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &gps_global_origin_topic);
if (!success) {
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1634,7 +1634,7 @@ void AP_DDS_Client::write_goal_topic()
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size);
const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic);
if (!success) {
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}
@ -1651,7 +1651,7 @@ void AP_DDS_Client::write_status_topic()
const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
}
}
}

2
libraries/AP_DDS/AP_DDS_config.h
View File

@ -161,7 +161,7 @@
#ifndef AP_DDS_DEFAULT_UDP_IP_ADDR
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.13.2"
#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.144.2"
#else
#define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1"
#endif

18
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -1953,6 +1953,24 @@ void AP_DroneCAN::logging(void)
return;
}
const auto &s = *stats;
// @LoggerMessage: CANS
// @Description: CAN Bus Statistics
// @Field: TimeUS: Time since system startup
// @Field: I: driver index
// @Field: T: transmit success count
// @Field: Trq: transmit request count
// @Field: Trej: transmit reject count
// @Field: Tov: transmit overflow count
// @Field: Tto: transmit timeout count
// @Field: Tab: transmit abort count
// @Field: R: receive count
// @Field: Rov: receive overflow count
// @Field: Rer: receive error count
// @Field: Bo: bus offset error count
// @Field: Etx: ESC successful send count
// @Field: Stx: Servo successful send count
// @Field: Ftx: ESC/Servo failed-to-send count
AP::logger().WriteStreaming("CANS",
"TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx",
"s#-------------",

84
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -758,52 +758,52 @@ void AP_ESC_Telem::update()
telemdata.power_percentage);
}
#endif //AP_EXTENDED_ESC_TELEM_ENABLED
}
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
// Write an EDTv2 message, if there is any update
uint16_t edt2_status = telemdata.edt2_status;
uint16_t edt2_stress = telemdata.edt2_stress;
if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) {
// Could probably be faster/smaller with bitmasking, but not sure
uint8_t status = 0;
if (EDT2_HAS_NEW_DATA(edt2_stress)) {
status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA);
// Write an EDTv2 message, if there is any update
uint16_t edt2_status = telemdata.edt2_status;
uint16_t edt2_stress = telemdata.edt2_stress;
if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) {
// Could probably be faster/smaller with bitmasking, but not sure
uint8_t status = 0;
if (EDT2_HAS_NEW_DATA(edt2_stress)) {
status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA);
}
if (EDT2_HAS_NEW_DATA(edt2_status)) {
status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA);
}
if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) {
status |= uint8_t(log_Edt2_Status::ALERT_BIT);
}
if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) {
status |= uint8_t(log_Edt2_Status::WARNING_BIT);
}
if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) {
status |= uint8_t(log_Edt2_Status::ERROR_BIT);
}
// An EDT2 status message is:
// id: starts from 0
// stress: the current stress which comes from edt2_stress
// max_stress: the maximum stress which comes from edt2_status
// status: the status bits which come from both
const struct log_Edt2 pkt_edt2{
LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)),
time_us : now_us64,
instance : i,
stress : EDT2_STRESS_FROM_STRESS(edt2_stress),
max_stress : EDT2_STRESS_FROM_STATUS(edt2_status),
status : status,
};
if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) {
// Only clean the telem_updated bits if the write succeeded.
// This is important because, if rate limiting is enabled,
// the log-on-change behavior may lose a lot of entries
telemdata.edt2_status &= ~EDT2_TELEM_UPDATED;
telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED;
}
}
if (EDT2_HAS_NEW_DATA(edt2_status)) {
status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA);
}
if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) {
status |= uint8_t(log_Edt2_Status::ALERT_BIT);
}
if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) {
status |= uint8_t(log_Edt2_Status::WARNING_BIT);
}
if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) {
status |= uint8_t(log_Edt2_Status::ERROR_BIT);
}
// An EDT2 status message is:
// id: starts from 0
// stress: the current stress which comes from edt2_stress
// max_stress: the maximum stress which comes from edt2_status
// status: the status bits which come from both
const struct log_Edt2 pkt_edt2{
LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)),
time_us : now_us64,
instance : i,
stress : EDT2_STRESS_FROM_STRESS(edt2_stress),
max_stress : EDT2_STRESS_FROM_STATUS(edt2_status),
status : status,
};
if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) {
// Only clean the telem_updated bits if the write succeeded.
// This is important because, if rate limiting is enabled,
// the log-on-change behavior may lose a lot of entries
telemdata.edt2_status &= ~EDT2_TELEM_UPDATED;
telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED;
}
}
#endif // AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
}
}
}
#endif // HAL_LOGGING_ENABLED

12
libraries/AP_Filesystem/README.md
View File

@ -70,11 +70,12 @@ parameter. The format is:
```
uint8_t type:4; // AP_Param type NONE=0, INT8=1, INT16=2, INT32=3, FLOAT=4
uint8_t flags:4; // for future use
uint8_t flags:4; // bit 0: default value included, bits 1-3: for future use
uint8_t common_len:4; // number of name bytes in common with previous entry, 0..15
uint8_t name_len:4; // non-common length of param name -1 (0..15)
uint8_t name[name_len]; // name
uint8_t data[]; // value, length given by variable type
uint8_t default[]; // optional default value, included if flags bit 0 is set
```
There may be any number of leading zero pad bytes before the start of
@ -91,6 +92,10 @@ file.
The name_len field is the number of non-common characters, minus one.
The default value is included only if requested by the withdefaults
query string and if different from the set value. Otherwise, flags
bit 0 will not be set, and default will be of zero length.
### Query Strings
The file name @PARAM/param.pck can optionally be extended with query
@ -101,6 +106,11 @@ string elements to change the result. For example:
that means to download 10 parameters starting with parameter number
50.
- @PARAM/param.pck?withdefaults=1
that means to include the default values in the returned data, where
it is different from the parameter's set value.
### Parameter Client Examples
The script Tools/scripts/param_unpack.py can be used to unpack a

16
libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp
View File

@ -46,17 +46,17 @@ private:
bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
{
if (sector > 1) {
AP_HAL::panic("FATAL: write to sector %u\n", (unsigned)sector);
AP_HAL::panic("FATAL: write to sector %u", (unsigned)sector);
}
if (offset + length > flash_sector_size) {
AP_HAL::panic("FATAL: write to sector %u at offset %u length %u\n",
AP_HAL::panic("FATAL: write to sector %u at offset %u length %u",
(unsigned)sector,
(unsigned)offset,
(unsigned)length);
}
uint8_t *b = &flash[sector][offset];
if ((offset & 1) || (length & 1)) {
AP_HAL::panic("FATAL: invalid write at %u:%u len=%u\n",
AP_HAL::panic("FATAL: invalid write at %u:%u len=%u",
(unsigned)sector,
(unsigned)offset,
(unsigned)length);
@ -66,7 +66,7 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data
const uint16_t v = le16toh_ptr(&data[i*2]);
uint16_t v2 = le16toh_ptr(&b[i*2]);
if (v & !v2) {
AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n",
AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x",
(unsigned)sector,
unsigned(offset+i),
b[i],
@ -74,7 +74,7 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data
}
#ifndef AP_FLASHSTORAGE_MULTI_WRITE
if (v != v2 && v != 0xFFFF && v2 != 0xFFFF) {
AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n",
AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x",
(unsigned)sector,
unsigned(offset+i),
b[i],
@ -90,10 +90,10 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data
bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
{
if (sector > 1) {
AP_HAL::panic("FATAL: read from sector %u\n", (unsigned)sector);
AP_HAL::panic("FATAL: read from sector %u", (unsigned)sector);
}
if (offset + length > flash_sector_size) {
AP_HAL::panic("FATAL: read from sector %u at offset %u length %u\n",
AP_HAL::panic("FATAL: read from sector %u at offset %u length %u",
(unsigned)sector,
(unsigned)offset,
(unsigned)length);
@ -105,7 +105,7 @@ bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint1
bool FlashTest::flash_erase(uint8_t sector)
{
if (sector > 1) {
AP_HAL::panic("FATAL: erase sector %u\n", (unsigned)sector);
AP_HAL::panic("FATAL: erase sector %u", (unsigned)sector);
}
memset(&flash[sector][0], 0xFF, flash_sector_size);
return true;

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1619,7 +1619,7 @@ void AP_GPS::handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_
*/
bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt)
{
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
if (static_cast<int>(chan) >= MAVLINK_COMM_NUM_BUFFERS) {
return false;
}
if (rtcm.parsers[chan] == nullptr) {

2
libraries/AP_Generator/AP_Generator.cpp
View File

@ -13,6 +13,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("Os")
#include "AP_Generator.h"
#if HAL_GENERATOR_ENABLED

2
libraries/AP_Generator/AP_Generator_Backend.cpp
View File

@ -12,6 +12,8 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("Os")
#include "AP_Generator_Backend.h"
#if HAL_GENERATOR_ENABLED

2
libraries/AP_Generator/AP_Generator_IE_2400.cpp
View File

@ -13,6 +13,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("Os")
#include "AP_Generator_IE_2400.h"
#if AP_GENERATOR_IE_2400_ENABLED

2
libraries/AP_Generator/AP_Generator_IE_650_800.cpp
View File

@ -13,6 +13,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("Os")
#include "AP_Generator_IE_650_800.h"
#if AP_GENERATOR_IE_650_800_ENABLED

2
libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp
View File

@ -13,6 +13,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("Os")
#include "AP_Generator_IE_FuelCell.h"
#if AP_GENERATOR_IE_ENABLED

2
libraries/AP_Generator/AP_Generator_RichenPower.cpp
View File

@ -13,6 +13,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("Os")
#include "AP_Generator_config.h"
#if AP_GENERATOR_RICHENPOWER_ENABLED

4
libraries/AP_HAL/CANIface.h
View File

@ -273,8 +273,8 @@ protected:
#ifndef HAL_BOOTLOADER_BUILD
HAL_Semaphore sem;
#endif
// allow up to 2 callbacks per interface
FrameCb cb[2];
// allow up to 3 callbacks per interface
FrameCb cb[3];
} callbacks;
uint32_t bitrate_;

5
libraries/AP_HAL/DSP.cpp
View File

@ -286,6 +286,11 @@ float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* rea
float y1 = real_fft[k_max-1];
float y2 = real_fft[k_max];
float y3 = real_fft[k_max+1];
if (is_zero(y2) || is_zero(y1)) {
return 0.0f;
}
float d = 0.0f;
if (y1 > y3) {

3
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -1329,9 +1329,6 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len)
*/
void RCOutput::cork(void)
{
if (corked) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
corked = true;
#if HAL_WITH_IO_MCU
if (iomcu_enabled) {

4
libraries/AP_HAL_ChibiOS/UARTDriver.cpp
View File

@ -191,7 +191,7 @@ void UARTDriver::thread_rx_init(void)
uart_rx_thread,
nullptr);
if (uart_rx_thread_ctx == nullptr) {
AP_HAL::panic("Could not create UART RX thread\n");
AP_HAL::panic("Could not create UART RX thread");
}
}
}
@ -209,7 +209,7 @@ void UARTDriver::thread_init(void)
uart_thread_trampoline,
this);
if (uart_thread_ctx == nullptr) {
AP_HAL::panic("Could not create UART TX thread\n");
AP_HAL::panic("Could not create UART TX thread");
}
}
}

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