Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Debug AGL. Enable VL53L0X for Sparky2 on EXT I2C

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-11-15 14:00:04 +10:00
parent 2c65d2273e
commit 5f8113e5aa
4 changed files with 15 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

@ -53,5 +53,6 @@ typedef enum {
DEBUG_RANGEFINDER,
DEBUG_RANGEFINDER_QUALITY,
DEBUG_PITOT,
DEBUG_AGL,
DEBUG_COUNT
} debugType_e;

2
src/main/fc/settings.yaml
View file

@ -66,7 +66,7 @@ tables:
- name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT"]
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator

10
src/main/navigation/navigation_pos_estimator.c
View file

@ -966,7 +966,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
}
}
else {
posEstimator.est.aglVel = 0;
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z - posEstimator.est.aglOffset;
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
}
#else
@ -1008,6 +1009,13 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
updateActualAGLAndClimgRate(false, false, posEstimator.est.aglAlt, 0);
}
#if defined(NAV_BLACKBOX)
DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
#endif
/* Store history data */
posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos;
posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel;

6
src/main/target/SPARKY2/target.h
View file

@ -109,7 +109,7 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
//#define I2C_DEVICE_EXT (I2CDEV_2)
#define I2C_DEVICE_EXT (I2CDEV_2)
#define USE_ADC
// PC2 shared with HC-SR04
@ -124,7 +124,9 @@
#define LED_STRIP
#define LED_STRIP_TIMER TIM5
// #define USE_RANGEFINDER
#define USE_RANGEFINDER
#define USE_RANGEFINDER_VL53L0X
#define RANGEFINDER_VL53L0X_INSTANCE I2C_DEVICE_EXT
// #define USE_RANGEFINDER_HCSR04
// #define RANGEFINDER_HCSR04_TRIGGER_PIN PC2
// #define RANGEFINDER_HCSR04_ECHO_PIN PC3