Commit Graph

17655 Commits

Author SHA1 Message Date
Andrew Tridgell
1de946b29e AP_NavEKF2: default EK2_MAG_EF_LIM to 50
this was supposed to be part of the original PR (agreed with Paul to
enable by default)
2019-07-06 15:50:34 +10:00
Andrew Tridgell
994a4b2082 AP_NavEKF2: constrain field by table after fusion
this should make for faster convergence
2019-07-06 15:50:34 +10:00
Andrew Tridgell
6f5d0fa3f0 AP_NavEKF2: added EK2_MAG_EF_LIM parameter
this sets a limit on the difference between the earth field from the
WMM tables and the learned earth field inside the EKF. Setting it to
zero disables the feature. A positive value sets the limit in mGauss.
2019-07-06 15:50:34 +10:00
Andrew Tridgell
74805870b8 AP_Compass: use new get_earth_field_ga() API 2019-07-06 15:50:34 +10:00
Andrew Tridgell
7814a3f2e3 AP_Declination: added get_earth_field_ga() interface
this gives a more convenient API for getting the earth field as a
vector
2019-07-06 15:50:34 +10:00
Andrew Tridgell
659458f208 AP_NavEKF3: don't use disabled gyros in opticalflow takeoff detection 2019-07-06 15:50:34 +10:00
Andrew Tridgell
14265d9580 AP_NavEKF2: don't use disabled gyros in opticalflow takeoff detection 2019-07-06 15:50:34 +10:00
Andrew Tridgell
91889d22cd AP_Mount: don't use disabled IMUs in solo gimbal code 2019-07-06 15:50:34 +10:00
Andrew Tridgell
1f309e8401 AP_AHRS: only use enabled IMUs in DCM
honor the INS_USE parameters for DCM
2019-07-06 15:50:34 +10:00
Andrew Tridgell
7a9203ef29 AP_InertialSensor: zero accumulators on time step
this zeros the delta angle and delta velocity accumulators when a
sensor is unavailable for a period of 0.1s. This prevents garbage
values being passed into the EKF when a sensor dies and then becomes
available again some time later
2019-07-06 15:50:34 +10:00
Andrew Tridgell
a5e3cd664b AP_InertialSensor: added RC switch for killing IMUs 2019-07-06 15:50:34 +10:00
Andrew Tridgell
d0792f29da HAL_ChibiOS: cope with calling delay_microseconds_boost() multiple times
needed for updated IMU wait code
2019-07-06 10:03:18 +10:00
Andrew Tridgell
0bbe9bfb71 AP_InertialSensor: try much harder to get all IMU samples
this we ensures we get new data for all active IMUs on each loop,
rather than sometimes returning with some IMUs not having data.

This matters as not having a sample on an IMU for a single loop can
cause an EKF IMU failover, which will degrade the learned bias
variances

The issue is usually only seen under high load, such as requesting a
loop rate beyond what the hardware is capable of
2019-07-06 10:03:18 +10:00
Andrew Tridgell
47f9cce612 AP_NavEKF3: added inactive bias learning
this allows for biases on inactive IMUs to be learned so that an IMU
switch within an EKF lane does not cause a jump in the IMU data
2019-07-06 08:34:31 +10:00
Andrew Tridgell
9f4085b0bc AP_NavEKF2: learn gyro biases for inactive gyros
this allows us to learn the gyro biases each lane would need if it had
to switch to another gyro due to a sensor failure. This prevents a
sudden change in gyro bias on IMU failure
2019-07-06 08:33:59 +10:00
Andrew Tridgell
a91af12364 AP_GPS: fixed u-blox F9 auto configuration
# Conflicts:
#	libraries/AP_GPS/AP_GPS_UBLOX.cpp
2019-07-06 08:33:15 +10:00
Andrew Tridgell
1cd90e5ae7 AP_GPS_UBLOX: add support for TIMEGPS message. used to get gps week 2019-06-29 10:05:03 +10:00
Andrew Tridgell
a03b3051fc HAL_ChibiOS: fixed LEDs on new CUAVv5 revision
# Conflicts:
#	libraries/AP_HAL_ChibiOS/hwdef/CUAVv5/hwdef.dat
2019-05-03 17:32:32 +10:00
Andrew Tridgell
c51b433c28 HAL_ChibiOS: fixed LEDs on CUAVv5Nano
need to be opendrain
2019-05-03 17:31:45 +10:00
Andrew Tridgell
376031c5f4 AP_Notify: don't use pinMode for pixracer LEDs
this is needed for LEDs that are driven by 5V, which means they need
to use OPENDRAIN.
2019-05-03 17:31:41 +10:00
Jaaaky
715f117d4b AP_InertialSensor: fix accelcalsimple watchdog 2019-05-03 10:37:34 +10:00
Andrew Tridgell
2094ffbf28 HAL_ChibiOS: fixed brick2 valid status in POWR flags for fmuv5
and fixed sense of VBUS
2019-04-26 07:13:11 +10:00
Andrew Tridgell
7acbcd88f8 AP_BoardConfig: add targetted check for cube black internal sensors
# Conflicts:
#	libraries/AP_BoardConfig/board_drivers.cpp
2019-04-24 12:31:33 +10:00
Jonathan Challinger
e29d03a994 AP_Baro: make crc4 a static member of AP_Baro_MS56xx 2019-04-24 12:31:02 +10:00
Jonathan Challinger
2b6ca186a8 AP_HAL_ChibiOS: add define HAL_CHIBIOS_ARCH_CUBEBLACK to cubeblack hwdef 2019-04-24 12:30:59 +10:00
Michael du Breuil
b236e80e41 AP_GPS: Log reciever status to GPS SD card
This improves log analysis of bad GPS health messages when you have a
report of bad GPS health but lost the MAVLink message, or didn't have
access to the autopilot log with the information for some reason.
2019-04-23 11:07:04 +10:00
Andrew Tridgell
1bfa79b744 AP_GPS: Remove external event from GPS data stream
If the event pin floats, this can lead to UART congestion, causing the
EKF to reject the GPS data, and the vehicle will drift around the sky in
a most disturbing manner

# Conflicts:
#	libraries/AP_GPS/AP_GPS_SBF.cpp
2019-04-23 11:06:59 +10:00
Andrew Tridgell
3680d8f1fa HAL_PX4: added empty flash driver 2019-04-22 15:58:01 +10:00
Andrew Tridgell
ca37b06af2 AP_InertialSensor: don't set INS_ENABLE_MASK to found IMU mask 2019-04-22 14:47:43 +10:00
Andrew Tridgell
a53480f904 AP_AHRS: ensure AHRS never uses an unhealthy gyro 2019-04-22 14:35:11 +10:00
Andrew Tridgell
087d20b22c HAL_ChibiOS: mark SDIO and SDMMC IO as pullup
this is needed for boards such as PixhackV3 which don't have hardware
pullups. Thanks to Hongle for reporting this.
2019-04-22 11:35:36 +10:00
Andrew Tridgell
5d0e785e73 AP_Arming: make arm_checks() virtual 2019-04-22 11:11:45 +10:00
Andrew Tridgell
6d3160b473 HAL_ChibiOS: fixed merge errors with hal.flash usage 2019-04-22 09:43:25 +10:00
Andrew Tridgell
33908f06a5 HAL_ChibiOS: use hal.flash API
# Conflicts:
#	libraries/AP_HAL_ChibiOS/Storage.cpp
#	libraries/AP_HAL_ChibiOS/Util.cpp
2019-04-22 09:16:26 +10:00
Andrew Tridgell
c77e4e6ec1 HAL_ChibiOS: fixed iomcu build with flash driver 2019-04-22 09:05:17 +10:00
Andrew Tridgell
ac1136d633 HAL_SITL: added empty flash driver 2019-04-22 09:05:01 +10:00
Andrew Tridgell
ca3f26f33a HAL_Linux: added empty flash driver 2019-04-22 09:04:58 +10:00
Andrew Tridgell
b7286ee03b HAL_Empty: added empty flash driver 2019-04-22 09:04:45 +10:00
Andrew Tridgell
35d711093b HAL_ChibiOS: implement flash driver 2019-04-22 09:03:05 +10:00
Andrew Tridgell
463bb6609e AP_HAL: added hal.flash driver
needed for semaphore
2019-04-22 09:02:49 +10:00
Andrew Tridgell
cd3881823b AP_HAL: added save/restore of attitude in backup registers 2019-04-21 13:08:46 +10:00
Andrew Tridgell
f8e5ef38e8 HAL_ChibiOS: added save/restore of attitude in backup registers 2019-04-21 13:08:46 +10:00
Andrew Tridgell
49d82efdc5 AP_AHRS: added save/restore of attitude in backup registers 2019-04-21 13:08:29 +10:00
Andrew Tridgell
2a7f0e4c47 HAL_ChibiOS: fixed build with older gcc 2019-04-20 20:29:51 +10:00
Andrew Tridgell
9f0312d744 AP_AHRS: save/restore home to backup registers
restore on watchdog reset
2019-04-20 19:50:25 +10:00
Andrew Tridgell
723a7bb647 HAL_ChibiOS: save/restore home position in backup registers 2019-04-20 19:49:46 +10:00
Andrew Tridgell
186f36a5bd AP_HAL: added save/restore of home to backup registers 2019-04-20 19:49:11 +10:00
Andrew Tridgell
4139450db8 AP_InertialSensor: prevent watchdog in accelcal 2019-04-20 17:35:49 +10:00
Andrew Tridgell
6e80174147 HAL_ChibiOS: allow for delay in bootloader flash 2019-04-20 16:33:27 +10:00
Andrew Tridgell
f89aadd2ba HAL_ChibiOS: fixed watchdog timeout setting 2019-04-20 16:31:28 +10:00
Andrew Tridgell
337cc16880 GCS_MAVLink: added method to lockup autopilot
used for watchdog testing
2019-04-20 15:17:55 +10:00
Andrew Tridgell
6252f97df4 AP_Logger: prevent log erase from triggering watchdog
# Conflicts:
#	libraries/DataFlash/DataFlash_File.cpp
2019-04-20 14:16:22 +10:00
Andrew Tridgell
b6285547c3 AP_Logger: force logging on an armed watchdog reset
and don't clear sdcard space
2019-04-20 14:16:22 +10:00
Andrew Tridgell
f5e170c76b HAL_ChibiOS: implement was_watchdog_armed() 2019-04-20 14:16:22 +10:00
Andrew Tridgell
63e636fd5a AP_HAL: added was_watchdog_armed()
allow decisions to be based on whether this is a watchdog reset and we
were armed
2019-04-20 14:16:22 +10:00
Andrew Tridgell
ff3527eef5 AP_BoardConfig: auto-restore safety state on watchdog reset 2019-04-20 14:16:22 +10:00
Andrew Tridgell
0a263cf202 HAL_ChibiOS: added ability to restore safety state on watchdog reset
# Conflicts:
#	libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
2019-04-20 13:59:39 +10:00
Andrew Tridgell
187ced5115 AP_HAL: added was_watchdog_safety_off()
used to restore safety state on watchdog reset
2019-04-20 13:59:01 +10:00
Andrew Tridgell
ddda455973 HAL_ChibiOS: change to 2s timeout on watchdog
a bit more of a safety net against false positives for stable release
2019-04-20 13:58:58 +10:00
Andrew Tridgell
3597e77f49 HAL_ChibiOS: fixed watchdog on H7 2019-04-20 13:57:43 +10:00
Andrew Tridgell
025496f65e AP_Compass: flag compass cal as long expected delay 2019-04-20 13:57:28 +10:00
Andrew Tridgell
cec6f0e3d4 HAL_ChibiOS: implement scheduler->expect_delay_ms()
# Conflicts:
#	libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
#	libraries/AP_HAL_ChibiOS/Scheduler.h
2019-04-20 13:57:23 +10:00
Andrew Tridgell
4e66449b52 AP_Logger: handle long delays in opening log files 2019-04-20 13:55:37 +10:00
Andrew Tridgell
3d386f333f AP_HAL: added expect_delay_ms() to Scheduler
used to notify scheduler of long expected delay in main thread
2019-04-20 13:55:33 +10:00
Andrew Tridgell
fed89a0b16 HAL_ChibiOS: added commented out test code for watchdog 2019-04-20 13:55:27 +10:00
Andrew Tridgell
3cb134b67c AP_Baro: skip cal on watchdog reset 2019-04-20 13:55:21 +10:00
Andrew Tridgell
7e675805fa AP_Airspeed: skip cal on watchdog reset 2019-04-20 13:55:17 +10:00
Andrew Tridgell
3f0af2ce29 AP_InertialSensor: skip gyro cal on watchdog reset 2019-04-20 13:55:08 +10:00
Andrew Tridgell
ad2ff5a207 HAL_ChibiOS: implement was_watchdog_reset()
# Conflicts:
#	libraries/AP_HAL_ChibiOS/Util.h
2019-04-20 13:55:03 +10:00
Andrew Tridgell
4b4eb33558 AP_HAL: added was_watchdog_reset()
# Conflicts:
#	libraries/AP_HAL/Util.h
2019-04-20 13:54:41 +10:00
Andrew Tridgell
ac5294c974 HAL_ChibiOS: record reason for reset
allows us to tell if reset was due to watchdog
2019-04-20 13:54:18 +10:00
Andrew Tridgell
5823a6b963 HAL_ChibiOS: use BRD_OPTIONS to enable watchdog
# Conflicts:
#	libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
2019-04-20 13:54:14 +10:00
Andrew Tridgell
348506b3c4 AP_BoardConfig: added BRD_OPTIONS
used to enable STM32 watchdog

# Conflicts:
#	libraries/AP_BoardConfig/AP_BoardConfig.cpp
#	libraries/AP_BoardConfig/AP_BoardConfig.h
2019-04-20 13:53:45 +10:00
Andrew Tridgell
78ac3e1e2f HAL_ChibiOS: added IWDG watchdog support
this resets the MCU if the main loop stops for 1 second

# Conflicts:
#	libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
2019-04-20 13:52:31 +10:00
Andrew Tridgell
ae2d4e6edc HAL_ChibiOS: change CUAVv5Nano default PWM count to 11 2019-04-11 16:12:32 +10:00
Andrew Tridgell
021816ecbc HAL_ChibiOS: fixed CUAVv5Nano uarts 2019-04-11 16:12:29 +10:00
Andrew Tridgell
ee9d084757 HAL_ChibiOS: support for CUAVv5Nano board
F765 with no IOMCU
2019-04-11 16:12:27 +10:00
Andrew Tridgell
1420265513 HAL_ChibiOS: fixed fallback to microSD for storage
this is used when FRAM fails on a board with microSD support. The
double init caused the validate() in the FATFS code to fail
2019-04-07 13:40:24 +10:00
Andrew Tridgell
f50a6b59cc AP_TECS: fixed a bug in changes from rate-limited to non-limited airspeed
The calculation of the non-limited airspeed rate demand used the last
non-limited airspeed, whereas it should have used the last adjusted
value. This led to a single frame spike in airspeed demand, which fed
through to a sudden change in pitch integrator.
2019-04-01 09:29:25 +11:00
Andrew Tridgell
b1a8055f20 AP_TECS: prevent airspeed demand spikes causing large pitch changes
a short term spike in the derivative of speed demand could cause the
constraint on the pitch integrator to push the pitch integrator to
very low values, causing a sharp nose down which takes a long time to
recover from
2019-04-01 09:29:25 +11:00
Andrew Tridgell
0360f887a0 HAL_ChibiOS: allocate threads from any heap
this fixes a failure on MatekF405-Wing where it fails to allocate the
SPI thread for the IMU
2019-02-27 12:00:18 +11:00
Andrew Tridgell
d71c8bc7ce AP_Compass: change to 3 compasses in SITL 2019-02-25 10:52:25 +11:00
Rob Ratcliff
199412adf2 RC_Channel: treat UINT16_MAX as a value of 0 in set_override
Addresses ignore R/C override issue documented here:
https://github.com/ArduPilot/ardupilot/issues/1667
2019-02-25 10:52:25 +11:00
Randy Mackay
8d3e1e45a7 AC_PosControl: remove unnecessary init of last_update_xy_ms 2019-02-25 10:52:25 +11:00
Andrew Tridgell
f03893c2f8 HAL_ChibiOS: setup APJ board IDs in environment 2019-02-25 10:52:25 +11:00
Andrew Tridgell
2187ee4611 HAL_ChibiOS: fixed mini-pix uart order 2019-02-25 10:52:25 +11:00
Andrew Tridgell
659e8395d5 AP_NavEKF3: fixed EKF compass switching
when we had 3 compasses the lack of the 'break' meant when we switched
compass in flight we would always switch back instantly to the one
that we had just rejected.
2019-02-25 10:52:25 +11:00
Andrew Tridgell
fe724e40b3 AP_NavEKF2: fixed EKF compass switching
when we had 3 compasses the lack of the 'break' meant when we switched
compass in flight we would always switch back instantly to the one
that we had just rejected.
2019-02-25 10:52:25 +11:00
Andrew Tridgell
037469db30 AC_WPNav: prevent I term buildup during landing
this prevents I term buildup in the XY velocity controller during
landing. This to account for the EKF giving a non-zero horizontal
velocity when we have touched down. The I term buildup in the XY
velocity controller can lead to the attitude error going above the
level for disabling the relax function as the throttle mix is
changed. That results in large motor outputs which can tip over the
vehicle after touchdown.

Thanks to Leonard for the suggestion
2019-02-25 10:52:25 +11:00
Andrew Tridgell
4b33865ee2 AP_Math: fixed inefficient sq() function 2019-02-25 10:52:25 +11:00
Andrew Tridgell
51acb72152 AP_RangeFinder: fixed lightware serial with LW20 lidar
it needs a longer serial write to force it to serial mode from i2c
2019-02-25 10:52:25 +11:00
Andrew Tridgell
3dd4cf4d87 AP_Rangefinder: probe all I2C buses for rangefinders 2019-02-25 10:52:25 +11:00
Andrew Tridgell
a2f0ff7386 Filter: fixed crash on zero cutoff frequency
this fixes #10435
2019-02-25 10:52:25 +11:00
Andrew Tridgell
ba53230f70 HAL_ChibiOS: fixed CAN on Pixhawk4 and PH4-mini
the silent pins floating disabled CAN
2019-01-23 13:26:55 +11:00
Andrew Tridgell
98fa9454ed HAL_ChibiOS: disable USART6_TX on fmuv5
this prevents it acting as a pullup on SBUS input for Pixhawk4. Thanks
to David Sidrane for the suggestion.

This also enables the extra 3 PWMs for PH4-mini
2019-01-18 09:45:46 +11:00
Andrew Tridgell
7040b6c2d7 AP_RCProtocol: on DSM sync error don't reset channel count
this fixes the issue noticed by Andreyl in 3.6.5rc1
2019-01-15 10:41:50 +11:00
Andrew Tridgell
e33d42e659 SITL: added simulated battery for quadplane
needed for motor interference for mag
2019-01-15 10:41:50 +11:00
Michael du Breuil
0a7fc2fd12 AP_HAL_ChibiOS: Fix FMUv2 LED lighting up 2019-01-11 17:46:20 +11:00
Andrew Tridgell
81b5c3a7da HAL_ChibiOS: fixed sdcard build 2019-01-11 13:25:32 +11:00
Andrew Tridgell
12be040da2 AP_BoardConfig: fixed build 2019-01-11 13:15:36 +11:00