Skip to content

Commit

Permalink
update codebase of ardupilot (#1)
Browse files Browse the repository at this point in the history
* AP_Motors: Add a seprate spool down time, if it's 0 use spool up time

* Tools: add extract_features Check Firmware and ODID

* Tools: add build_option - ODID (OpenDroneID/RemoteID)

* AP_InertialSensor: add INS_RAW_LOG_OPT to allow raw logging of post, and pre+post on primary or all gyros

* AP_Mount: Xacti does not report cannot take pic

* AP_Mount: Xacti take pic reliability improved

* AP_Mount: Xacti uses GCS_SEND_TEXT

* AP_Mount: Xacti get_param_name_str returns empty string on failure

* AC_PID: Simplify update_error method by calling update all

* Plane: fix guided heading control anti windup

* Plane: MAV_CMD_GUIDED_CHANGE_HEADING: allow changing heading type or rate for same heading

* Plane: GuidedHeading PID: remove slew limit

* AP_EFI: Rate limit the megasquirt driver

This fixes it up so that the driver actually works on things like
AP_Periph that poll at a high rate. This was never a problem with the
main firmware as EFI was run at a lower rate, but on AP_Periph this was
much to fast. This lead to spamming fresh requests and keeping the
buffer completly stuffed with requests. To compound it, the EFI device
would start over when there was a fresh request, and eventually our
buffer writes become corrupted leading to bad checksums, and a complete
failure of the comms. This prevents that situation from happening.

* AP_RCProtocol: disable raw logging if no RC_Channel

We're asking the singleton here for information on whether we should do raw logging.  We may not actually be compiling the RC_Channel library in.

* GCS_MAVLink: handle message interval commands as both long and int

* Tools: handle message interval commands as both long and int

* AP_Scripting: winch-test param name typo fix

* AP_Scripting: winch-test becomes winch-control applet

* AP_Scripting: winch-control gets .md file

* AP_Winch: minor comment improvement

* AP_Winch: Daiwa gets stuck protection

* AP_Mount: Xacti minor format update

* AP_Mount: Xacti supports optical zoom

* AP_Mount: Xacti digital zoom percentage fix

* Sub: accept MISSION_START as both long and int

* autotest: add test for Sub MAV_CMD_MISSION_START

* autotest: add upload_rally_points_from_locations

* AP_HAL: Add support for NeoPixelRGB

* AP_HAL_ChibiOS: add support for NeoPixelRGB

* AP_SerialLED: add support for NeoPixelRGB via set_num_neopixel_rgb()

* AP_Notify: add support for NeoPixelRGB via set_num_neopixel_rgb()

* AP_Scripting: support set_num_neopixel_rgb()

* AP_OSD: Make per-cell voltage be shown to two decimal places again

* AP_Common: make Location.cpp compile without AP::ahrs() available

* AP_AHRS: allow compilation when GPS not available

* GCS_MAVLink: allow compilation when GPS library not available

* hwdef: HerePro requires AHRS to compile

* AP_Periph: instantiate AP_AHRS even if not in SITL

Closes potential problem with HerePro if it calls AP_AHRS methods

* AP_TemperatureSensor: support var pointer backend params

* AP_TempratureSensor: add support for analog sensor with polynomial

* AP_Scripting: added INF_Inject EFI driver

* AP_Scripting: add basic print

* Sub: accept DO_CHANGE_SPEED as both command_long and command_int

* autotest: add (disabled) sub test for MAV_CMD_DO_CHANGE_SPEED

this doesn't work on master either...

* AP_OLC: use right type and static const variable

* AP_OLC: add unit tests

* GCS_MAVLink: handle DO_AUX_FUNCTION as both long and int

* autotest: test MAV_CMD_DO_AUX_FUNCTION as both COMMAND_LONG and COMMAND_INT

* bootloaders: refresh Pixhawk6X bootloader

... and include the elf file so we can try to tell what's in the bin

* Plane: accept MAV_CMD_DO_LAND_START as both command_long and COMMAND_INT

* Tools: add tests for MAV_CMD_DO_LAND_START for both int and long

* ArduCopter: don't send fence_status mavlink message if fence not compiled in

* ArduPlane: don't send fence_status mavlink message if fence not compiled in

* ArduSub: don't send fence_status mavlink message if fence not compiled in

* Blimp: don't send fence_status mavlink message if fence not compiled in

* Rover: don't send fence_status mavlink message if fence not compiled in

* Copter: Two processes in one

* Tracker: enable EK2 and EK3 parameters

* Tools: board_types.txt: add Vimdrones board types

* SITL: document airspeed params

* Tools: document airspeed params

* AP_Avoidance: Change division to multiplication

* hwdef: ARKV6X ADIS16507 example

* AC_Fence: Change the description to match the actual value(NFC)

* AP_Math: add CRC8_generic method

* AP_Proximity: add driver for LD06

* AP_Proximity: Minor fixes to LD06 driver

* bootloaders: update CubeOrange bootloader to include flash-from-sd support

this is just a rebuild of this firmware; the changes have been in the hwdef for a very long time

* SITL: fix typos

* bootloaders: freshen CubeOrangePlus bootloader

notably this includes flash-from-SD-card support

* hwdef: CubeOrangePlus: build abin files

... for use with flash-from-SD-card

* GCS_MAVLink: cope with NaNs being passed in when doing conversion to command_int

* autotest: fix interact

* Tools: add InterActTest to show interaction with automated setup

* hwdef:add VTX power control to SpeedyBeeF405-Wing

* HAL_ChibiOS: Fix stm32l4+ flash issue

stm32l4+ flash reset register has been reset properly

* AP_GPS: have AP_GPS_UBLOX  use boolean uart read

* AP_InertialSensor: fixed the error value for BMI088

the bad value is -32768 not 0xffff (which is -1)

-32768 badly corrupts the low-pass filter, and is what we see in logs
(a large negative spike on all 3 axes)

update to bug fix from:
ArduPilot#23033

* AP_BattMonitor: added SHUNT parameter to INS2xx driver

some vendors want different shunt resistors

* Tools: allow Ohm units in parameters

* Copter: remove duplicate friend declaration

Signed-off-by: Rhys Mainwaring <[email protected]>

* AP_Winch: Make healthy strings common

* Filter: HarmonicNotch: convert harmonics to int32 param

* AP_InertialSensor: call init for harmonic notch params

* AP_GyroFFT: update harmonics to uint32

* AP_AHRS: add airspeed estimate status logging

* ArduPlane: add airspeed estimate status logging

* hwdef: add MatekL431 ADSB Periph

* Tools: add bootloader for MatekL431-ADSB

* Filter: LowPassFilter2p: constrain cuttoff to 40% of  sample rate

* GCS_MAVLink: allow PREFLIGHT_STORAGE as COMMAND_INT and COMMAND_LONG

* ArduSub: accept MAV_CMD_CONDITION_YAW as both long and int in Sub

* Tools: accept MAV_CMD_CONDITION_YAW as both long and int in Sub

* AP_GPS: correct placement of voiding clause

should be voided when no logging, not just on periph...

* AP_RangeFinder: log signal quality

* AP_RangeFinder: add quality to Blue Robotics Ping1D driver

* AP_RangeFinder_MAVLink: implement get_signal_quality_pct

Co-authored-by:  Willian Galvani <[email protected]>

* AP_RangeFinder: change get_signal_quality signature to use int8_t

* GCS_Common: use int8_t for rangefinder quality_pct

* AP_ChibiOS: Delete the same definition

* AP_DDS: use ROS convention for node name

Signed-off-by: Rhys Mainwaring <[email protected]>

* autotest: add autotests for MAV_CMD_SET_SOURCE_SET mavlink command

* GCS_MAVLink: handle EKF_SOURCE_SET as both long and int

* ArduPlane: allow Plane to run MAV_CMD_MISSION_START as long and int

* Tools: allow Plane to run MAV_CMD_MISSION_START as long and int

* AP_AHRS: Correct/clarify AHRS_WIND_MAX description

* autotest: don't build examples for fmuv2

needs extra defines as we kill features from it

* hwdef: Create correct README and add defaults to ease setup

* AP_RTC: add AP_RTC_config.h

* AP_ADSB: allow for compilation without AP_RTC_ENABLED

* AP_BoardConfig: add and use an AP_RTC_config.h

* AP_HAL_ChibiOS: disable RTC on periphs

the singleton isn't instantiated

* AP_Hott_Telem: allow for compilation without AP_RTC_ENABLED

* AP_Logger: allow for compilation without AP_RTC_ENABLED

* AP_Mount: allow for compilation without AP_RTC_ENABLED

* AP_MSP: add and use an AP_RTC_config.h

* AP_NMEA_Output: allow for compilation without AP_RTC_ENABLED

* AP_OSD: allow for compilation without AP_RTC_ENABLED

* AP_RCTelemetry: allow for compilation without AP_RTC_ENABLED

* AP_Stats: add and use an AP_RTC_config.h

* ArduCopter: allow for compilation without AP_RTC_ENABLED

* ArduPlane: allow for compilation without AP_RTC_ENABLED

* ArduSub: allow for compilation without AP_RTC_ENABLED

* GCS_MAVLink: allow for compilation without AP_RTC_ENABLED

* Rover: allow for compilation without AP_RTC_ENABLED

* AP_RTC: remove code if AP_RTC_ENABLED is false

* AP_GPS: allow for compilation without AP_RTC_ENABLED

* waf: disable RTC on sitl_periph_gps

* AP_HAL_ChibiOS: add and use HAP_PERIPH_ENABLE_RTC

* Tools: add and use HAP_PERIPH_ENABLE_RTC

* AP_HAL_ChibiOS: add RTC support to GPS periphs that log

this re-adds support for RTC into GPS peripherals that also log.  This was removed as these boards were calling methods on the nullptr

* AP_HAL_ChibiOS: JFB110 board definition


Co-authored-by: Randy Mackay <[email protected]>

* AP_DroneCAN: fixed sim on hw build

tested using:

Tools/scripts/sitl-on-hardware/sitl-on-hw.py --board Pixhawk6C --vehicle plane --simclass Plane --frame plane

* AC_AttitudeControl: Fix some typos

Fixed some typos found in the code.

* AC_Autorotation: Fix some typos

Fixed some typos found in the code.

* AC_AutoTune: Fix some typos

Fixed some typos found in the code.

* AC_Avoidance: Fix some typos

Fixed some typos found in the code.

* AC_CustomControl: Fix some typos

Fixed some typos found in the code.

* AC_Fence: Fix some typos

Fixed some typos found in the code.

* AC_PID: Fix some typos

Fixed some typos found in the code.

* AC_PrecLand: Fix some typos

Fixed some typos found in the code.

* AC_Sprayer: Fix some typos

Fixed some typos found in the code.

* AC_WPNav: Fix some typos

Fixed some typos found in the code.

* AP_AccelCal: Fix some typos

Fixed some typos found in the code.

* AP_ADSB: Fix some typos

Fixed some typos found in the code.

* AP_AHRS: Fix some typos

Fixed some typos found in the code.

* AP_Airspeed: Fix some typos

Fixed some typos found in the code.

* AP_AIS: Fix some typos

Fixed some typos found in the code.

* AP_Arming: Fix some typos

Fixed some typos found in the code.

* AP_Baro: Fix some typos

Fixed some typos found in the code.

* AP_BattMonitor: Fix some typos

Fixed some typos found in the code.

* AP_Beacon: Fix some typos

Fixed some typos found in the code.

* AP_BLHeli: Fix some typos

Fixed some typos found in the code.

* AP_BoardConfig: Fix some typos

Fixed some typos found in the code.

* AP_Camera: Fix some typos

Fixed some typos found in the code.

* AP_CANManager: Fix some typos

Fixed some typos found in the code.

* AP_Common: Fix some typos

Fixed some typos found in the code.

* AP_Compass: Fix some typos

Fixed some typos found in the code.

* AP_DAL: Fix some typos

Fixed some typos found in the code.

* AP_DDS: Fix some typos

Fixed some typos found in the code.

* AP_DroneCAN: Fix some typos

Fixed some typos found in the code.

* AP_EFI: Fix some typos

Fixed some typos found in the code.

* AP_ESC_Telem: Fix some typos

Fixed some typos found in the code.

* AP_ExternalAHRS: Fix some typos

Fixed some typos found in the code.

* AP_Filesystem: Fix some typos

Fixed some typos found in the code.

* AP_FlashIface: Fix some typos

Fixed some typos found in the code.

* AP_FlashStorage: Fix some typos

Fixed some typos found in the code.

* AP_Follow: Fix some typos

Fixed some typos found in the code.

* AP_Frsky_Telem: Fix some typos

Fixed some typos found in the code.

* AP_Generator: Fix some typos

Fixed some typos found in the code.

* AP_GPS: Fix some typos

Fixed some typos found in the code.

* AP_Gripper: Fix some typos

Fixed some typos found in the code.

* AP_GyroFFT: Fix some typos

Fixed some typos found in the code.

* AP_HAL: Fix some typos

Fixed some typos found in the code.

* AntennaTracker: Fix some typos

Fixed some typos found in the code.

* ArduCopter: Fix some typos

Fixed some typos found in the code.

* ArduPlane: Fix some typos

Fixed some typos found in the code.

* ArduSub: Fix some typos

Fixed some typos found in the code.

* Blimp: Fix some typos

Fixed some typos found in the code.

* GCS_MAVLink: accept accel cal vehicle positions as both int and long

* AP_RCProtocol: protect against invalid data in SBUS

* AP_RCProtocol: make sbus_decode public to allow for a test suite

* AP_SBusOut: make sbus output exactly match sbus input decoding

* HAL_Linux: use the AP_RCProtocol sbus decoder for Linux

* AP_RCProtocol: added a test suite for SBUS encode/decode

cover all values and check special handling of 875

* Tools: rebuild IO firmware

* AP_RCProtocol: prevent decoding past end of output array

this fixes test_sbus on clang

* AP_Scripting: added INS filters to revert list

these may be changed in a tuning session

* AP_Logger: added build directory to VER message

this allows log review tools to use right parameter and mode map when vendor has changed the
vehicle type strings

* hwdef: correct compilation of CubeOrange-SimOnHW

../../libraries/AP_InertialSensor/AP_InertialSensor_config.h:20:2: error: #error "INS_AUX_INSTANCES must be zero if INS_MAX_INSTANCES is less than 3"
   20 | #error "INS_AUX_INSTANCES must be zero if INS_MAX_INSTANCES is less than 3"
      |  ^~~~~

* Rover: 4.4.0-beta8 release notes

* Copter: 4.4.2-beta1 release notes

* Rover: update 4.4.2-beta1 release notes

* Copter: update 4.4.2-beta1 release notes

* AP_Bootloader: add SpeedyBeeF405v4

* AP_OpenDroneID: ensure Persistent memory is not read continuously

* AP_OpenDroneID: only load from persistent memory in init()

we do not want to do this from update() as it is an expensive call

* AP_OpenDroneID: remove duplicate definition of AP_OPENDRONEID_ENABLED

... my guess is conflict resolution caused this

* autotest: added Plane.TerrainRally test

reproduces the issue from
ArduPilot#25157

* Plane: fixed terrain RTL with rally points

this fixes a bug where if the terrain database cache does not have the
tile for the location of a rally point then RTL to the rally point
with TERRAIN_FOLLOW=1 will not track terrain

The underlying issue is that Location::loc.change_alt_frame() will
return false if the location is not in the terrain memory cache. We
can't just extrapolate as the rally point could be in a totally
different terrain area to the current location. So instead we set it
as terrain_following_pending and fix it as soon as the terrain cache
is filled.

fixes ArduPilot#25157

* Plane: 4.4.2-beta1 release notes

* hwdef: Cite CubeOrange-SimOnHW as the default file

* Tools: adjust install_prereqs_ubuntu.sh to handle Mantic

* Vagrant: add Mantic Minotaur to available VMs

* github: test mantic not bionic

bionic no longer builds  SITL:
../../libraries/AP_Logger/AP_Logger_Backend.cpp: In member function 'bool AP_Logger_Backend::Write_VER()':
../../libraries/AP_Logger/AP_Logger_Backend.cpp:577:5: sorry, unimplemented: non-trivial designated initializers not supported
     };
     ^
compilation terminated due to -Wfatal-errors.

* autotest: correct hook removal for Copter tests

these hooks were remaining active if the test failed

* GCS_MAVlink: correct routing for Solo Gimbal

Check for a opro camera in a Solo gimbal added and re-enable the routing of Gopro Mavlink commands

* Flter: notchfilter: remove unneeded value and pre-multiply for speed

* AP_Periph: Rate limit EFI driver updates

Also remove the last update variables for features that were compiled
out.

* AC_PID: If PD max limiting isn't active clear the flag

* AP_HAL_ChibiOS: hwdef: scripts: defaults_periph: disable unused libaries by defualt

* AP_Param: Allow override of AP_PARAM_DYNAMIC_ENABLED define

* AP_Scripting: don't include CAN support on periph if there is only one CAN port

* AP_Scripting: require a file system

* AP_Scripting: lua_scripts: add missing HAL_LOGGING_ENABLED define

* AP_Scripting: generator: emit dependancys for operators

* AP_Scripting: Generator: allow unused enum loader function

* AP_Scripting: add lots off missing binding dependencies

* AP_Scripting: add support for dependencty on manual methods, remove handling of mission commands without AP_Mission

* GCS_MAVLink: handle MAV_CMD_DEBUG_TRAP as both long and int

* SRV_Channel: expose public function to convert scaled value to pwm

Allow other modules to get and store the PWM value for a specific scaled
value and re-apply it later.

* AP_ServoRelayEvents: allow mavlink command of rcin scaled functions

Allow `MAV_CMD_DO_SET_SERVO` and `MAV_CMD_DO_REPEAT_SERVO` to be used on
a servo output set to an RCINnScaled function (i.e. k_rcinN_mapped).

Scaling is applied so that a commanded servo PWM of <=1000 maps to
SERVOn_MIN, a PWM of 1500 maps to SERVOn_TRIM, and a PWM of >=2000 maps to
SERVOn_MAX. Linear interpolation is performed between ranges.

* GCS_MAVLink: add build option for deprecated MISSION_REQUEST

replaced by MISSION_REQUEST_INT

* autotest: test setting modes via COMMAND_INT and DO_SET_MODE

* GCS_MAVLink: allow setting modes via COMMAND_INT and DO_SET_MODE

* AP_BattMonitor: DroneCAN: always have consumed energy

* ArduPlane: accept LOITER_UNLIM and RETURN_TO_LAUNCH as command_int

* Tools: accept LOITER_UNLIM and RETURN_TO_LAUNCH as command_int

* Plane:force min transition timer to 2 sec

* hwdef:fix bug in AtomRCF405NAVI,fix some bds that aren't building

* AP_Beacon: fix example when Beacon not available

simply print the fact that the beacon librar y isn't compiled in

* Tools: Replay: correct compilation when AdvancedFailsafe not available

* AP_Scripting: added SVFFI generator support

* autotest: add test for SET_ATTITUDE_TARGET headings

* Plane:reset in landing seq on mode change while disarmed

* Copter: encapsulate auto takeoff into an ojbect

similar to the encapsulation of "user takeoff" into an object

* AP_HAL_ChibiOS: correct printing of timer information

* Blimp: remove misleading implementation of MAV_CMD_CONDITION_YAW

this would return success when it, in fact, does nothing.

* AP_HAL_ChibiOS: add support for saving and restoring SCK pin state
when SPI goes into undefined state during reset

* AP_HAL_ChibiOS: fix mode setting for ICM45686 on CubeOrangePlus

* AP_HAL_ChibiOS: add option to set HAL_SPI_SCK_SAVE_RESTORE

* autotest: add explicit test for MAV_CMD_GET_HOME_POSITION

* GCS_MAVLink: handle MAV_CMD_GET_HOME_POSITION as both long and int

* autotest: add test for MAV_CMD_NAV_TAKEOFF for quadplane using command_long

* Plane: use origin-relative altitudes rather than home-relative

* autotest: allow more time for disarm on vtol-state test

right on the edge

* autotest: test DO_SEND_BANNER as both long and int

* GCS_MAVLink: handle MAV_CMD_DO_SEND_BANNER as both long and int

* hwdef: Adapt the rotation direction of the QMC5883L

* hwdef: To adapt the rotation directionof the QMC5883L on new Aerofox-Airspeed-DLVR

---------

Signed-off-by: Rhys Mainwaring <[email protected]>
Co-authored-by: Michael du Breuil <[email protected]>
Co-authored-by: Tom Pittenger <[email protected]>
Co-authored-by: Iampete1 <[email protected]>
Co-authored-by: Randy Mackay <[email protected]>
Co-authored-by: Peter Barker <[email protected]>
Co-authored-by: Andy Piper <[email protected]>
Co-authored-by: Michelle Rossouw <[email protected]>
Co-authored-by: Andrew Tridgell <[email protected]>
Co-authored-by: Pierre Kancir <[email protected]>
Co-authored-by: muramura <[email protected]>
Co-authored-by: Huibean <[email protected]>
Co-authored-by: Anthony Luo <[email protected]>
Co-authored-by: alexklimaj <[email protected]>
Co-authored-by: Joshua Henderson <[email protected]>
Co-authored-by: Adithya Patil <[email protected]>
Co-authored-by: rishabsingh3003 <[email protected]>
Co-authored-by: Henry Wurzburg <[email protected]>
Co-authored-by: thu5cob <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>
Co-authored-by: Willian Galvani <[email protected]>
Co-authored-by: Clyde McQueen <[email protected]>
Co-authored-by: Tim Tuxworth <[email protected]>
Co-authored-by: jfbblue0922 <[email protected]>
Co-authored-by: Mykhailo Kuznietsov <[email protected]>
Co-authored-by: bugobliterator <[email protected]>
Co-authored-by: stephan <[email protected]>
Co-authored-by: Thomas Watson <[email protected]>
Co-authored-by: AerofoxTech <[email protected]>
  • Loading branch information
1 parent 8c4fb26 commit 4e28931
Show file tree
Hide file tree
Showing 439 changed files with 12,315 additions and 3,705 deletions.
6 changes: 3 additions & 3 deletions .github/workflows/test_environment.yml
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,14 @@ jobs:
fail-fast: false # don't cancel if a job from the matrix fails
matrix:
include:
- os: ubuntu
name: bionic
- os: ubuntu
name: focal
- os: ubuntu
name: jammy
- os: ubuntu
name: lunar
- os: ubuntu
name: mantic
- os: archlinux
name: latest
- os: debian
Expand Down Expand Up @@ -77,7 +77,7 @@ jobs:
with:
submodules: 'recursive'
- name: test install environment ${{matrix.os}}.${{matrix.name}}
timeout-minutes: 30
timeout-minutes: 45
env:
DISABLE_MAVNATIVE: True
DEBIAN_FRONTEND: noninteractive
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_size.yml
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ jobs:
config: [
Durandal,
MatekF405,
KakuteF7,
MatekH743-bdshot,
Pixhawk1-1M,
MatekF405-CAN, # see special "build bootloader" code below
DrotekP3Pro, # see special "build bootloader" code below
Expand Down
4 changes: 2 additions & 2 deletions AntennaTracker/AntennaTracker.txt
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ around. It might even damage itself.
AntennaTracker (like other ardupilot software such as ArduPlane, ArduRover
etc) has configuration values that control and tailor its operation, and which
are stored in EEPROM on the processor. The configuration is restored from
EEPROM every time the processsor starts.
EEPROM every time the processor starts.

You can use MissionPlanner, mavproxy or apm_planner or other mavlink compatible
software to check and change the configuration of your AntennaTracker.
Expand Down Expand Up @@ -219,7 +219,7 @@ the vehicle, cd to the ArduPlane directory and run this:

../Tools/autotest/sim_arduplane.sh -T --aircraft test

The -T flag tells sim_arduplane.sh to start an entenna tracker
The -T flag tells sim_arduplane.sh to start an antenna tracker
simulator and also start a virtual antenna tracker in a window.

To start the antenna tracker running run "tracker start" in the
Expand Down
2 changes: 1 addition & 1 deletion AntennaTracker/GCS_Mavlink.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK
protected:

// telem_delay is not used by Tracker but is pure virtual, thus
// this implementaiton. it probably *should* be used by Tracker,
// this implementation. it probably *should* be used by Tracker,
// as currently Tracker may brick XBees
uint32_t telem_delay() const override { return 0; }

Expand Down
14 changes: 13 additions & 1 deletion AntennaTracker/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ const AP_Param::Info Tracker::var_info[] = {

// @Param: ONOFF_PITCH_MINT
// @DisplayName: Pitch minimum movement time
// @Description: Minimim amount of time in seconds to move in pitch
// @Description: Minimum amount of time in seconds to move in pitch
// @Units: s
// @Increment: 0.01
// @Range: 0 2
Expand Down Expand Up @@ -541,6 +541,18 @@ const AP_Param::Info Tracker::var_info[] = {
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
GOBJECT(logger, "LOG", AP_Logger),

#if HAL_NAVEKF2_AVAILABLE
// @Group: EK2_
// @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2),
#endif

#if HAL_NAVEKF3_AVAILABLE
// @Group: EK3_
// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
#endif

AP_VAREND
};

Expand Down
2 changes: 2 additions & 0 deletions AntennaTracker/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,8 @@ class Parameters {
k_param_disarm_pwm,

k_param_auto_opts,
k_param_NavEKF2,
k_param_NavEKF3,

k_param_logger = 253, // 253 - Logging Group

Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/AP_ExternalControl_Copter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f
}
const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;

// Copter velocity is positive if aicraft is moving up which is opposite the incoming NED frame.
// Copter velocity is positive if aircraft is moving up which is opposite the incoming NED frame.
Vector3f velocity_NEU_ms {
linear_velocity.x,
linear_velocity.y,
Expand Down
3 changes: 2 additions & 1 deletion ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,8 @@ class Copter : public AP_Vehicle {
friend class ModeZigZag;
friend class ModeAutorotate;
friend class ModeTurtle;
friend class AP_ExternalControl_Copter;

friend class _AutoTakeoff;

Copter(void);

Expand Down
58 changes: 30 additions & 28 deletions ArduCopter/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -511,7 +511,9 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
MSG_GPS2_RAW,
MSG_GPS2_RTK,
MSG_NAV_CONTROLLER_OUTPUT,
#if AP_FENCE_ENABLED
MSG_FENCE_STATUS,
#endif
MSG_POSITION_TARGET_GLOBAL_INT,
};
static const ap_message STREAM_POSITION_msgs[] = {
Expand Down Expand Up @@ -795,6 +797,33 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
return handle_MAV_CMD_DO_WINCH(packet);
#endif

case MAV_CMD_NAV_LOITER_UNLIM:
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;

case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;

case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_LAND:
if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;

#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_DO_LAND_START:
if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
#endif

default:
return GCS_MAVLINK::handle_command_int_packet(packet, msg);
}
Expand Down Expand Up @@ -838,33 +867,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
return MAV_RESULT_ACCEPTED;
}

#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_DO_LAND_START:
if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
#endif

case MAV_CMD_NAV_LOITER_UNLIM:
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;

case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;

case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_LAND:
if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;

default:
return GCS_MAVLINK::handle_command_long_packet(packet, msg);
}
Expand Down Expand Up @@ -1478,7 +1480,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
} // end handle mavlink


MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_long_t &packet) {
MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
#if ADVANCED_FAILSAFE == ENABLED
if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) {
return MAV_RESULT_ACCEPTED;
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/GCS_Mavlink.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK

uint32_t telem_delay() const override;

MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override;
MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override;

uint8_t sysid_my_gcs() const override;
bool sysid_enforce() const override;
Expand Down
22 changes: 22 additions & 0 deletions ArduCopter/ReleaseNotes.txt
Original file line number Diff line number Diff line change
@@ -1,5 +1,27 @@
ArduPilot Copter Release Notes:
------------------------------------------------------------------
Copter 4.4.2-beta1 13-Oct-2023
Changes from 4.4.1
1) Autopilot related enhancements and fixes
- BETAFPV-F405 support
- MambaF405v2 battery and serial setup corrected
- mRo Control Zero OEM H7 bdshot support
- SpeedyBee-F405-Wing gets VTX power control
- SpeedyBee-F405-Mini support
- T-Motor H743 Mini support
2) EKF3 supports baroless boards
3) GPS-for-yaw allows base GPS to update at only 3Hz
4) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT)
5) Log VER message includes vehicle type
6) OpenDroneId option to auto-store IDs in persistent flash
7) RC SBUS protection against invalid data in first 4 channels
8) Bug fixes
- BMI088 IMU error value handling fixed to avoid occasional negative spike
- Dev environment CI autotest stability improvements
- OSD correct DisplayPort BF MSP symbols
- OSD option to correct direction arrows for BF font set
- Sensor status reporting to GCS fixed for baroless boards
------------------------------------------------------------------
Copter 4.4.1 26-Sep-2023 / 4.4.1-beta2 14-Sep-2023
Changes from 4.4.1-beta1
1) Autopilot related enhancements
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/crash_check.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ void Copter::thrust_loss_check()
{
static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed

// no-op if suppresed by flight options param
// no-op if suppressed by flight options param
if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) {
return;
}
Expand Down Expand Up @@ -181,7 +181,7 @@ void Copter::thrust_loss_check()
// check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors
void Copter::yaw_imbalance_check()
{
// no-op if suppresed by flight options param
// no-op if suppressed by flight options param
if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) != 0) {
return;
}
Expand Down
39 changes: 24 additions & 15 deletions ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,27 @@ class ParametersG2;

class GCS_Copter;

// object shared by both Guided and Auto for takeoff.
// position controller controls vehicle but the user can control the yaw.
class _AutoTakeoff {
public:
void run();
void start(float complete_alt_cm, bool terrain_alt);
bool get_position(Vector3p& completion_pos);

bool complete; // true when takeoff is complete

private:
// altitude above-ekf-origin below which auto takeoff does not control horizontal position
bool no_nav_active;
float no_nav_alt_cm;

// auto takeoff variables
float complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
bool terrain_alt; // true if altitudes are above terrain
Vector3p complete_pos; // target takeoff position as offset from ekf origin in cm
};

class Mode {

public:
Expand Down Expand Up @@ -51,6 +72,8 @@ class Mode {
// do not allow copying
CLASS_NO_COPY(Mode);

friend class _AutoTakeoff;

// returns a unique number specific to this mode
virtual Number mode_number() const = 0;

Expand Down Expand Up @@ -215,21 +238,7 @@ class Mode {

virtual bool do_user_takeoff_start(float takeoff_alt_cm);

// method shared by both Guided and Auto for takeoff.
// position controller controls vehicle but the user can control the yaw.
void auto_takeoff_run();
void auto_takeoff_start(float complete_alt_cm, bool terrain_alt);
bool auto_takeoff_get_position(Vector3p& completion_pos);

// altitude above-ekf-origin below which auto takeoff does not control horizontal position
static bool auto_takeoff_no_nav_active;
static float auto_takeoff_no_nav_alt_cm;

// auto takeoff variables
static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain
static bool auto_takeoff_complete; // true when takeoff is complete
static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm
static _AutoTakeoff auto_takeoff;

public:
// Navigation Yaw control
Expand Down
20 changes: 12 additions & 8 deletions ArduCopter/mode_auto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -350,7 +350,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
pos_control->init_z_controller();

// initialise alt for WP_NAVALT_MIN and set completion alt
auto_takeoff_start(alt_target_cm, alt_target_terrain);
auto_takeoff.start(alt_target_cm, alt_target_terrain);

// set submode
set_submode(SubMode::TAKEOFF);
Expand All @@ -364,7 +364,7 @@ bool ModeAuto::wp_start(const Location& dest_loc)
Vector3f stopping_point;
if (_mode == SubMode::TAKEOFF) {
Vector3p takeoff_complete_pos;
if (auto_takeoff_get_position(takeoff_complete_pos)) {
if (auto_takeoff.get_position(takeoff_complete_pos)) {
stopping_point = takeoff_complete_pos.tofloat();
}
}
Expand Down Expand Up @@ -536,7 +536,7 @@ bool ModeAuto::is_landing() const

bool ModeAuto::is_taking_off() const
{
return ((_mode == SubMode::TAKEOFF) && !auto_takeoff_complete);
return ((_mode == SubMode::TAKEOFF) && !auto_takeoff.complete);
}

// auto_payload_place_start - initialises controller to implement a placing
Expand Down Expand Up @@ -965,7 +965,7 @@ void ModeAuto::takeoff_run()
if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) {
copter.set_auto_armed(true);
}
auto_takeoff_run();
auto_takeoff.run();
}

// auto_wp_run - runs the auto waypoint controller
Expand Down Expand Up @@ -1326,13 +1326,13 @@ void ModeAuto::payload_place_run()
FALLTHROUGH;

case PayloadPlaceStateType_Ascent_Start: {
auto_takeoff_start(nav_payload_place.descent_start_altitude_cm, false);
auto_takeoff.start(nav_payload_place.descent_start_altitude_cm, false);
nav_payload_place.state = PayloadPlaceStateType_Ascent;
}
break;

case PayloadPlaceStateType_Ascent:
if (auto_takeoff_complete) {
if (auto_takeoff.complete) {
nav_payload_place.state = PayloadPlaceStateType_Done;
}
break;
Expand Down Expand Up @@ -1782,7 +1782,11 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
} else {
// absolute delay to utc time
#if AP_RTC_ENABLED
nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
#else
nav_delay_time_max_ms = 0;
#endif
}
gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
}
Expand Down Expand Up @@ -1970,13 +1974,13 @@ bool ModeAuto::verify_takeoff()
{
#if AP_LANDINGGEAR_ENABLED
// if we have reached our destination
if (auto_takeoff_complete) {
if (auto_takeoff.complete) {
// retract the landing gear
copter.landinggear.retract_after_takeoff();
}
#endif

return auto_takeoff_complete;
return auto_takeoff.complete;
}

// verify_land - returns true if landing has been completed
Expand Down
Loading

0 comments on commit 4e28931

Please sign in to comment.