mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 09:16:01 +03:00
Merge branch 'development' into failsafe-stage1-mods
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
commit
afe1e31be9
37 changed files with 512 additions and 465 deletions
31
docs/Cli.md
31
docs/Cli.md
|
|
@ -251,13 +251,39 @@ Re-apply any new defaults as desired.
|
|||
| blackbox_rate_num | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations |
|
||||
| blackbox_rate_denom | 1 | Blackbox logging rate denominator. See blackbox_rate_num. |
|
||||
| blackbox_device | SPIFLASH | Selection of where to write blackbox data |
|
||||
| ledstrip_visual_beeper | OFF | |
|
||||
| osd_video_system | 0 | |
|
||||
| osd_row_shiftdown | 0 | |
|
||||
| osd_units | 0 | |
|
||||
| osd_rssi_alarm | 20 | |
|
||||
| osd_cap_alarm | 2200 | |
|
||||
| osd_time_alarm | 10 | |
|
||||
| osd_alt_alarm | 100 | |
|
||||
| osd_main_voltage_pos | 0 | |
|
||||
| osd_rssi_pos | 0 | |
|
||||
| osd_flytimer_pos | 0 | |
|
||||
| osd_ontime_pos | 0 | |
|
||||
| osd_flymode_pos | 0 | |
|
||||
| osd_throttle_pos | 0 | |
|
||||
| osd_vtx_channel_pos | 0 | |
|
||||
| osd_crosshairs | 0 | |
|
||||
| osd_artificial_horizon | 0 | |
|
||||
| osd_current_draw_pos | 0 | |
|
||||
| osd_mah_drawn_pos | 0 | |
|
||||
| osd_craft_name_pos | 0 | |
|
||||
| osd_gps_speed_pos | 0 | |
|
||||
| osd_gps_sats_pos | 0 | |
|
||||
| osd_altitude_pos | 0 | |
|
||||
| osd_pid_roll_pos | 0 | |
|
||||
| osd_pid_pitch_pos | 0 | |
|
||||
| osd_pid_yaw_pos | 0 | |
|
||||
| osd_power_pos | 0 | |
|
||||
| magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
|
||||
| magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
|
||||
| magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. |
|
||||
| acczero_x | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
|
||||
| acczero_y | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
|
||||
| acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
|
||||
| ledstrip_visual_beeper | OFF | |
|
||||
| accgain_x | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
|
||||
| accgain_y | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
|
||||
| accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
|
||||
|
|
@ -287,7 +313,8 @@ Re-apply any new defaults as desired.
|
|||
| default_rate_profile | 0 | Default = profile number |
|
||||
| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you aquirre valid GPS fix. |
|
||||
| mag_hold_rate_limit | 90 | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
|
||||
| `mc_p_pitch` | 40 | Multicopter rate stabilisation P-gain for PITCH |
|
||||
| `mag_calibration_time` | 30 | Adjust how long time the Calibration of mag will last. |
|
||||
| `mc_p_pitch` | 40 | Multicopter rate stabilisation P-gain for PITCH |
|
||||
| `mc_i_pitch` | 30 | Multicopter rate stabilisation I-gain for PITCH |
|
||||
| `mc_d_pitch` | 23 | Multicopter rate stabilisation D-gain for PITCH |
|
||||
| `mc_p_roll` | 40 | Multicopter rate stabilisation P-gain for ROLL |
|
||||
|
|
|
|||
|
|
@ -517,7 +517,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_SONAR:
|
||||
#ifdef SONAR
|
||||
return feature(FEATURE_SONAR);
|
||||
return sensors(SENSOR_SONAR);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
@ -1135,7 +1135,7 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
blackboxCurrent->gyroADC[i] = gyro.gyroADC[i];
|
||||
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
|
||||
}
|
||||
|
||||
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
|
|
@ -1338,7 +1338,7 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", 100); //For compatibility reasons write rc_rate 100
|
||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.dev.scale));
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x", castFloatBytesToInt(1.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.dev.acc_1G);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
||||
|
|
|
|||
|
|
@ -87,7 +87,8 @@
|
|||
#define PG_MODE_ACTIVATION_OPERATOR_CONFIG 1003
|
||||
#define PG_OSD_CONFIG 1004
|
||||
#define PG_BEEPER_CONFIG 1005
|
||||
#define PG_INAV_END 1005
|
||||
#define PG_RANGEFINDER_CONFIG 1006
|
||||
#define PG_INAV_END 1006
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
//#define PG_OSD_FONT_CONFIG 2047
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ typedef struct gyroDev_s {
|
|||
extiCallbackRec_t exti;
|
||||
float scale; // scalefactor
|
||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||
int16_t gyroZero[XYZ_AXIS_COUNT];
|
||||
uint8_t lpf;
|
||||
gyroRateKHz_e gyroRateKHz;
|
||||
uint8_t mpuDividerDrops;
|
||||
|
|
|
|||
|
|
@ -267,20 +267,17 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
}
|
||||
#endif
|
||||
|
||||
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
|
||||
void mpuDetect(gyroDev_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
uint8_t sig;
|
||||
uint8_t inquiryResult;
|
||||
|
||||
// MPU datasheet specifies 30ms.
|
||||
delay(35);
|
||||
|
||||
#ifndef USE_I2C
|
||||
ack = false;
|
||||
sig = 0;
|
||||
uint8_t sig = 0;
|
||||
bool ack = false;
|
||||
#else
|
||||
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
|
||||
uint8_t sig;
|
||||
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
|
||||
#endif
|
||||
if (ack) {
|
||||
gyro->mpuConfiguration.read = mpuReadRegisterI2C;
|
||||
|
|
@ -290,33 +287,30 @@ mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
|
|||
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
|
||||
UNUSED(detectedSpiSensor);
|
||||
#endif
|
||||
|
||||
return &gyro->mpuDetectionResult;
|
||||
return;
|
||||
}
|
||||
|
||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||
|
||||
// If an MPU3050 is connected sig will contain 0.
|
||||
uint8_t inquiryResult;
|
||||
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
|
||||
inquiryResult &= MPU_INQUIRY_MASK;
|
||||
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
|
||||
gyro->mpuDetectionResult.sensor = MPU_3050;
|
||||
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
|
||||
return &gyro->mpuDetectionResult;
|
||||
return;
|
||||
}
|
||||
|
||||
sig &= MPU_INQUIRY_MASK;
|
||||
|
||||
if (sig == MPUx0x0_WHO_AM_I_CONST) {
|
||||
|
||||
gyro->mpuDetectionResult.sensor = MPU_60x0;
|
||||
|
||||
mpu6050FindRevision(gyro);
|
||||
} else if (sig == MPU6500_WHO_AM_I_CONST) {
|
||||
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
|
||||
}
|
||||
|
||||
return &gyro->mpuDetectionResult;
|
||||
return;
|
||||
}
|
||||
|
||||
void mpuGyroInit(gyroDev_t *gyro)
|
||||
|
|
|
|||
|
|
@ -169,8 +169,13 @@ typedef enum {
|
|||
MPU_65xx_I2C,
|
||||
MPU_65xx_SPI,
|
||||
MPU_9250_SPI,
|
||||
ICM_20601_SPI,
|
||||
ICM_20602_SPI,
|
||||
ICM_20608_SPI,
|
||||
ICM_20649_SPI,
|
||||
ICM_20679_SPI,
|
||||
ICM_20689_SPI
|
||||
} detectedMPUSensor_e;
|
||||
} mpuSensor_e;
|
||||
|
||||
typedef enum {
|
||||
MPU_HALF_RESOLUTION,
|
||||
|
|
@ -178,7 +183,7 @@ typedef enum {
|
|||
} mpu6050Resolution_e;
|
||||
|
||||
typedef struct mpuDetectionResult_s {
|
||||
detectedMPUSensor_e sensor;
|
||||
mpuSensor_e sensor;
|
||||
mpu6050Resolution_e resolution;
|
||||
} mpuDetectionResult_t;
|
||||
|
||||
|
|
@ -187,7 +192,7 @@ void mpuGyroInit(struct gyroDev_s *gyro);
|
|||
struct accDev_s;
|
||||
bool mpuAccRead(struct accDev_s *acc);
|
||||
bool mpuGyroRead(struct gyroDev_s *gyro);
|
||||
mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
|
||||
void mpuDetect(struct gyroDev_s *gyro);
|
||||
bool mpuCheckDataReady(struct gyroDev_s *gyro);
|
||||
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
|
||||
|
||||
|
|
|
|||
|
|
@ -91,23 +91,23 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
|
|||
/* Set hdma_tim instance */
|
||||
hdma_tim.Instance = WS2811_DMA_STREAM;
|
||||
|
||||
uint32_t channelAddress = 0;
|
||||
//uint32_t channelAddress = 0;
|
||||
switch (WS2811_TIMER_CHANNEL) {
|
||||
case TIM_CHANNEL_1:
|
||||
timDMASource = TIM_DMA_ID_CC1;
|
||||
channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
|
||||
//channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
|
||||
break;
|
||||
case TIM_CHANNEL_2:
|
||||
timDMASource = TIM_DMA_ID_CC2;
|
||||
channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
|
||||
//channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
|
||||
break;
|
||||
case TIM_CHANNEL_3:
|
||||
timDMASource = TIM_DMA_ID_CC3;
|
||||
channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
|
||||
//channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
|
||||
break;
|
||||
case TIM_CHANNEL_4:
|
||||
timDMASource = TIM_DMA_ID_CC4;
|
||||
channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
|
||||
//channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,8 +32,6 @@
|
|||
#include "pwm_rx.h"
|
||||
#include "pwm_mapping.h"
|
||||
|
||||
void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput);
|
||||
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
|
||||
|
||||
/*
|
||||
Configuration maps
|
||||
|
|
|
|||
|
|
@ -32,3 +32,7 @@ void pwmWriteServo(uint8_t index, uint16_t value);
|
|||
|
||||
void pwmDisableMotors(void);
|
||||
void pwmEnableMotors(void);
|
||||
struct timerHardware_s;
|
||||
void pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput);
|
||||
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
|
||||
|
||||
|
|
|
|||
|
|
@ -181,8 +181,9 @@ bool isMotorBrushed(uint16_t motorPwmRate)
|
|||
return (motorPwmRate > 500);
|
||||
}
|
||||
|
||||
void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto)
|
||||
void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput)
|
||||
{
|
||||
UNUSED(enableOutput);
|
||||
uint32_t timerMhzCounter;
|
||||
pwmWriteFuncPtr pwmWritePtr;
|
||||
|
||||
|
|
@ -221,8 +222,9 @@ void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui
|
|||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse)
|
||||
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
|
||||
{
|
||||
UNUSED(enableOutput);
|
||||
servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,12 +17,33 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "common/time.h"
|
||||
|
||||
#include "io.h"
|
||||
|
||||
#define RANGEFINDER_OUT_OF_RANGE (-1)
|
||||
|
||||
typedef struct rangefinder_s {
|
||||
struct rangefinderDev_s;
|
||||
|
||||
typedef struct rangefinderHardwarePins_s {
|
||||
ioTag_t triggerTag;
|
||||
ioTag_t echoTag;
|
||||
} rangefinderHardwarePins_t;
|
||||
|
||||
typedef void (*rangefinderOpInitFuncPtr)(void);
|
||||
typedef void (*rangefinderOpStartFuncPtr)(void);
|
||||
typedef int32_t (*rangefinderOpReadFuncPtr)(void);
|
||||
|
||||
typedef struct rangefinderDev_s {
|
||||
timeMs_t delayMs;
|
||||
int16_t maxRangeCm;
|
||||
|
||||
// these are full detection cone angles, maximum tilt is half of this
|
||||
int16_t detectionConeDeciDegrees; // detection cone angle as in device spec
|
||||
int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
|
||||
} rangefinder_t;
|
||||
|
||||
// function pointers
|
||||
rangefinderOpInitFuncPtr init;
|
||||
rangefinderOpStartFuncPtr update;
|
||||
rangefinderOpReadFuncPtr read;
|
||||
} rangefinderDev_t;
|
||||
|
|
|
|||
|
|
@ -48,8 +48,7 @@
|
|||
*
|
||||
*/
|
||||
|
||||
STATIC_UNIT_TESTED volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
|
||||
sonarHcsr04Hardware_t sonarHcsr04Hardware;
|
||||
static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
|
||||
|
||||
#ifdef USE_EXTI
|
||||
static extiCallbackRec_t hcsr04_extiCallbackRec;
|
||||
|
|
@ -75,46 +74,8 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb)
|
|||
}
|
||||
#endif
|
||||
|
||||
void hcsr04_set_sonar_hardware(void)
|
||||
void hcsr04_init(void)
|
||||
{
|
||||
#if !defined(UNIT_TEST)
|
||||
|
||||
#ifdef STM32F10X
|
||||
// enable AFIO for EXTI support
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
|
||||
#endif
|
||||
|
||||
#if defined(STM32F3) || defined(STM32F4)
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
|
||||
|
||||
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
|
||||
#endif
|
||||
|
||||
// trigger pin
|
||||
triggerIO = IOGetByTag(sonarHcsr04Hardware.triggerTag);
|
||||
IOInit(triggerIO, OWNER_SONAR, RESOURCE_OUTPUT, 0);
|
||||
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
|
||||
|
||||
// echo pin
|
||||
echoIO = IOGetByTag(sonarHcsr04Hardware.echoTag);
|
||||
IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT, 0);
|
||||
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
|
||||
|
||||
#ifdef USE_EXTI
|
||||
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
|
||||
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
|
||||
EXTIEnable(echoIO, true);
|
||||
#endif
|
||||
|
||||
#endif // UNIT_TEST
|
||||
}
|
||||
|
||||
void hcsr04_init(rangefinder_t *rangefinder)
|
||||
{
|
||||
rangefinder->maxRangeCm = HCSR04_MAX_RANGE_CM;
|
||||
rangefinder->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
|
||||
rangefinder->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -156,4 +117,48 @@ int32_t hcsr04_get_distance(void)
|
|||
}
|
||||
return distance;
|
||||
}
|
||||
|
||||
bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonarHardwarePins)
|
||||
{
|
||||
#ifdef STM32F10X
|
||||
// enable AFIO for EXTI support
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
|
||||
#endif
|
||||
|
||||
#if defined(STM32F3) || defined(STM32F4)
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
|
||||
|
||||
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
|
||||
#endif
|
||||
|
||||
// trigger pin
|
||||
triggerIO = IOGetByTag(sonarHardwarePins->triggerTag);
|
||||
IOInit(triggerIO, OWNER_SONAR, RESOURCE_OUTPUT, 0);
|
||||
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
|
||||
|
||||
// echo pin
|
||||
echoIO = IOGetByTag(sonarHardwarePins->echoTag);
|
||||
IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT, 0);
|
||||
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
|
||||
|
||||
#ifdef USE_EXTI
|
||||
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
|
||||
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
|
||||
EXTIEnable(echoIO, true);
|
||||
#endif
|
||||
|
||||
dev->delayMs = 100;
|
||||
dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
|
||||
dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
|
||||
dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
|
||||
|
||||
dev->init = &hcsr04_init;
|
||||
dev->update = &hcsr04_start_reading;
|
||||
dev->read = &hcsr04_get_distance;
|
||||
|
||||
/* FIXME: Do actual hardware detection - see if HC-SR04 is alive */
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,13 +17,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
typedef struct sonarHcsr04Hardware_s {
|
||||
ioTag_t triggerTag;
|
||||
ioTag_t echoTag;
|
||||
} sonarHcsr04Hardware_t;
|
||||
#include "drivers/rangefinder.h"
|
||||
|
||||
struct rangefinder_s;
|
||||
void hcsr04_set_sonar_hardware(void);
|
||||
void hcsr04_init(struct rangefinder_s *rangefinder);
|
||||
void hcsr04_start_reading(void);
|
||||
int32_t hcsr04_get_distance(void);
|
||||
bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonarHardwarePins);
|
||||
|
|
|
|||
|
|
@ -114,22 +114,8 @@ static uint8_t i2c_srf10_read_byte(uint8_t i2cRegister)
|
|||
return byte;
|
||||
}
|
||||
|
||||
bool srf10_detect()
|
||||
static void srf10_init(void)
|
||||
{
|
||||
const uint8_t value = i2c_srf10_read_byte(SRF10_READ_Unused);
|
||||
if (value == SRF10_READ_Unused_ReturnValue) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void srf10_init(rangefinder_t *rangefinder)
|
||||
{
|
||||
rangefinder->maxRangeCm = SRF10_MAX_RANGE_CM;
|
||||
rangefinder->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
|
||||
rangefinder->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
|
||||
// set up the SRF10 hardware for a range of 6m
|
||||
minimumFiringIntervalMs = SRF10_MinimumFiringIntervalFor600cmRangeMs;
|
||||
i2c_srf10_send_byte(SRF10_WRITE_MaxGainRegister, SRF10_COMMAND_SetGain_Max);
|
||||
i2c_srf10_send_byte(SRF10_WRITE_RangeRegister, SRF10_RangeValue6m);
|
||||
// initiate first ranging command
|
||||
|
|
@ -141,7 +127,7 @@ void srf10_init(rangefinder_t *rangefinder)
|
|||
* Start a range reading
|
||||
* Called periodically by the scheduler
|
||||
*/
|
||||
void srf10_start_reading(void)
|
||||
static void srf10_start_reading(void)
|
||||
{
|
||||
// check if there is a measurement outstanding, 0xFF is returned if no measurement
|
||||
const uint8_t revision = i2c_srf10_read_byte(SRF10_READ_SoftwareRevision);
|
||||
|
|
@ -154,7 +140,7 @@ void srf10_start_reading(void)
|
|||
srf10measurementCm = RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
}
|
||||
const uint32_t timeNowMs = millis();
|
||||
const timeMs_t timeNowMs = millis();
|
||||
if (timeNowMs > timeOfLastMeasurementMs + minimumFiringIntervalMs) {
|
||||
// measurement repeat interval should be greater than minimumFiringIntervalMs
|
||||
// to avoid interference between connective measurements.
|
||||
|
|
@ -166,8 +152,28 @@ void srf10_start_reading(void)
|
|||
/**
|
||||
* Get the distance that was measured by the last pulse, in centimeters.
|
||||
*/
|
||||
int32_t srf10_get_distance(void)
|
||||
static int32_t srf10_get_distance(void)
|
||||
{
|
||||
return srf10measurementCm;
|
||||
}
|
||||
|
||||
bool srf10Detect(rangefinderDev_t *dev)
|
||||
{
|
||||
const uint8_t value = i2c_srf10_read_byte(SRF10_READ_Unused);
|
||||
if (value == SRF10_READ_Unused_ReturnValue) {
|
||||
dev->delayMs = SRF10_MinimumFiringIntervalFor600cmRangeMs + 10; // set up the SRF10 hardware for a range of 6m + margin of 10ms
|
||||
dev->maxRangeCm = SRF10_MAX_RANGE_CM;
|
||||
dev->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
|
||||
dev->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
|
||||
|
||||
dev->init = &srf10_init;
|
||||
dev->update = &srf10_start_reading;
|
||||
dev->read = &srf10_get_distance;
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,9 +17,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
struct rangefinder_s;
|
||||
bool srf10_detect();
|
||||
void srf10_init(struct rangefinder_s *rangefinder);
|
||||
void srf10_start_reading(void);
|
||||
int32_t srf10_get_distance(void);
|
||||
|
||||
bool srf10Detect(rangefinderDev_t *dev);
|
||||
|
|
|
|||
|
|
@ -102,6 +102,7 @@ extern uint8_t __config_end;
|
|||
#include "sensors/diagnostics.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#include "telemetry/frsky.h"
|
||||
|
|
@ -131,8 +132,6 @@ static void cliBootlog(char *cmdline);
|
|||
static char cliBuffer[64];
|
||||
static uint32_t bufferIndex = 0;
|
||||
|
||||
static const char* const emptyName = "-";
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
// sync this with mixerMode_e
|
||||
static const char * const mixerNames[] = {
|
||||
|
|
@ -147,10 +146,10 @@ static const char * const mixerNames[] = {
|
|||
|
||||
// sync this with features_e
|
||||
static const char * const featureNames[] = {
|
||||
"RX_PPM", "VBAT", "UNUSED_1", "RX_SERIAL", "MOTOR_STOP",
|
||||
"SERVO_TILT", "SOFTSERIAL", "GPS", "UNUSED_3",
|
||||
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "UNUSED_2",
|
||||
"RX_PPM", "VBAT", "", "RX_SERIAL", "MOTOR_STOP",
|
||||
"SERVO_TILT", "SOFTSERIAL", "GPS", "",
|
||||
"", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
|
||||
"BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
|
||||
"SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", "OSD", NULL
|
||||
};
|
||||
|
|
@ -160,6 +159,7 @@ static const char * const featureNames[] = {
|
|||
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE"};
|
||||
// sync with accelerationSensor_e
|
||||
static const char * const lookupTableAccHardware[] = { "NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE"};
|
||||
#if (FLASH_SIZE > 64)
|
||||
// sync with baroSensor_e
|
||||
static const char * const lookupTableBaroHardware[] = { "NONE", "AUTO", "BMP085", "MS5611", "BMP280", "FAKE"};
|
||||
// sync with magSensor_e
|
||||
|
|
@ -169,7 +169,6 @@ static const char * const lookupTableRangefinderHardware[] = { "NONE", "HCSR04",
|
|||
// sync with pitotSensor_e
|
||||
static const char * const lookupTablePitotHardware[] = { "NONE", "AUTO", "MS4525", "FAKE"};
|
||||
|
||||
#if (FLASH_SIZE > 64)
|
||||
// sync this with sensors_e
|
||||
static const char * const sensorTypeNames[] = {
|
||||
"GYRO", "ACC", "BARO", "MAG", "SONAR", "PITOT", "GPS", "GPS+MAG", NULL
|
||||
|
|
@ -515,6 +514,11 @@ static const clivalue_t valueTable[] = {
|
|||
{ "accgain_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Y]) },
|
||||
{ "accgain_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) },
|
||||
|
||||
// PG_RANGEFINDER_CONFIG
|
||||
#ifdef SONAR
|
||||
{ "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
|
||||
#endif
|
||||
|
||||
// PG_COMPASS_CONFIG
|
||||
#ifdef MAG
|
||||
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) },
|
||||
|
|
@ -524,6 +528,7 @@ static const clivalue_t valueTable[] = {
|
|||
{ "magzero_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Y]) },
|
||||
{ "magzero_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Z]) },
|
||||
{ "mag_hold_rate_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN, MAG_HOLD_RATE_LIMIT_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hold_rate_limit) },
|
||||
{ "mag_calibration_time", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 30, 120 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magCalibrationTimeLimit) },
|
||||
#endif
|
||||
|
||||
// PG_BAROMETER_CONFIG
|
||||
|
|
@ -862,6 +867,10 @@ static const clivalue_t valueTable[] = {
|
|||
{ "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SPEED]) },
|
||||
{ "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SATS]) },
|
||||
{ "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ALTITUDE]) },
|
||||
{ "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_PIDS]) },
|
||||
{ "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PITCH_PIDS]) },
|
||||
{ "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_YAW_PIDS]) },
|
||||
{ "osd_power_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_POWER]) },
|
||||
#endif
|
||||
// PG_SYSTEM_CONFIG
|
||||
{ "i2c_overclock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, i2c_overclock) },
|
||||
|
|
@ -2569,6 +2578,8 @@ static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig,
|
|||
for (uint32_t i = 0; ; i++) { // disable all feature first
|
||||
if (featureNames[i] == NULL)
|
||||
break;
|
||||
if (featureNames[i][0] == '\0')
|
||||
continue;
|
||||
const char *format = "feature -%s\r\n";
|
||||
cliDefaultPrintf(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]);
|
||||
cliDumpPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]);
|
||||
|
|
@ -2576,6 +2587,8 @@ static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig,
|
|||
for (uint32_t i = 0; ; i++) { // reenable what we want.
|
||||
if (featureNames[i] == NULL)
|
||||
break;
|
||||
if (featureNames[i][0] == '\0')
|
||||
continue;
|
||||
const char *format = "feature %s\r\n";
|
||||
if (defaultMask & (1 << i)) {
|
||||
cliDefaultPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]);
|
||||
|
|
@ -2596,6 +2609,8 @@ static void cliFeature(char *cmdline)
|
|||
for (uint32_t i = 0; ; i++) {
|
||||
if (featureNames[i] == NULL)
|
||||
break;
|
||||
if (featureNames[i][0] == '\0')
|
||||
continue;
|
||||
if (mask & (1 << i))
|
||||
cliPrintf("%s ", featureNames[i]);
|
||||
}
|
||||
|
|
@ -2605,6 +2620,8 @@ static void cliFeature(char *cmdline)
|
|||
for (uint32_t i = 0; ; i++) {
|
||||
if (featureNames[i] == NULL)
|
||||
break;
|
||||
if (featureNames[i][0] == '\0')
|
||||
continue;
|
||||
cliPrintf("%s ", featureNames[i]);
|
||||
}
|
||||
cliPrint("\r\n");
|
||||
|
|
@ -2632,12 +2649,6 @@ static void cliFeature(char *cmdline)
|
|||
cliPrint("unavailable\r\n");
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#ifndef SONAR
|
||||
if (mask & FEATURE_SONAR) {
|
||||
cliPrint("unavailable\r\n");
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
if (remove) {
|
||||
featureClear(mask);
|
||||
|
|
|
|||
|
|
@ -47,6 +47,7 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/serial.h"
|
||||
|
|
@ -175,7 +176,7 @@ void validateAndFixConfig(void)
|
|||
{
|
||||
#if defined(SONAR)
|
||||
// FIXME: Completely disable sonar
|
||||
featureClear(FEATURE_SONAR);
|
||||
rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_NOTCH_1
|
||||
|
|
@ -306,13 +307,13 @@ void validateAndFixConfig(void)
|
|||
#endif
|
||||
|
||||
#if defined(NAZE) && defined(SONAR)
|
||||
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
|
||||
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
|
||||
featureClear(FEATURE_CURRENT_METER);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(OLIMEXINO) && defined(SONAR)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
|
||||
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
|
||||
featureClear(FEATURE_CURRENT_METER);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -325,20 +326,20 @@ void validateAndFixConfig(void)
|
|||
|
||||
#if defined(CC3D)
|
||||
#if defined(CC3D_PPM1)
|
||||
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
featureClear(FEATURE_SONAR);
|
||||
}
|
||||
#endif
|
||||
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
|
||||
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
|
||||
rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
|
||||
}
|
||||
#endif
|
||||
#else
|
||||
#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
|
||||
// shared pin
|
||||
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
|
||||
featureClear(FEATURE_SONAR);
|
||||
featureClear(FEATURE_SOFTSERIAL);
|
||||
featureClear(FEATURE_RSSI_ADC);
|
||||
}
|
||||
#endif
|
||||
#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
|
||||
// shared pin
|
||||
if (((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
|
||||
rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
|
||||
featureClear(FEATURE_SOFTSERIAL);
|
||||
featureClear(FEATURE_RSSI_ADC);
|
||||
}
|
||||
#endif
|
||||
#endif // CC3D_PPM1
|
||||
#endif // CC3D
|
||||
|
||||
|
|
|
|||
|
|
@ -49,8 +49,8 @@ typedef enum {
|
|||
FEATURE_SERVO_TILT = 1 << 5,
|
||||
FEATURE_SOFTSERIAL = 1 << 6,
|
||||
FEATURE_GPS = 1 << 7,
|
||||
FEATURE_UNUSED_3 = 1 << 8, // was FAILSAFE
|
||||
FEATURE_SONAR = 1 << 9,
|
||||
FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE
|
||||
FEATURE_UNUSED_4 = 1 << 9, // was FEATURE_SONAR
|
||||
FEATURE_TELEMETRY = 1 << 10,
|
||||
FEATURE_CURRENT_METER = 1 << 11,
|
||||
FEATURE_3D = 1 << 12,
|
||||
|
|
|
|||
|
|
@ -60,7 +60,6 @@
|
|||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/io.h"
|
||||
|
|
@ -101,7 +100,7 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
#include "sensors/initialisation.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
|
|
@ -242,14 +241,16 @@ void init(void)
|
|||
|
||||
#ifdef USE_SERVOS
|
||||
servosInit();
|
||||
mixerUpdateStateFlags(); // This needs to be called early to allow pwm mapper to use information about FIXED_WING state
|
||||
#endif
|
||||
|
||||
drv_pwm_config_t pwm_params;
|
||||
memset(&pwm_params, 0, sizeof(pwm_params));
|
||||
|
||||
#ifdef SONAR
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(batteryConfig()->currentMeterType);
|
||||
// HC-SR04 has a dedicated connection to FC and require two pins
|
||||
if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) {
|
||||
const rangefinderHardwarePins_t *sonarHardware = sonarGetHardwarePins();
|
||||
if (sonarHardware) {
|
||||
pwm_params.useSonar = true;
|
||||
pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag;
|
||||
|
|
@ -259,10 +260,7 @@ void init(void)
|
|||
#endif
|
||||
|
||||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
||||
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
|
||||
pwm_params.airplane = true;
|
||||
else
|
||||
pwm_params.airplane = false;
|
||||
pwm_params.airplane = STATE(FIXED_WING);
|
||||
#if defined(USE_UART2) && defined(STM32F10X)
|
||||
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
|
||||
#endif
|
||||
|
|
@ -392,14 +390,14 @@ void init(void)
|
|||
|
||||
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
|
||||
#if defined(FURYF3) || defined(OMNIBUS) || defined(SPRACINGF3MINI)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(SONAR) && defined(USE_SOFTSERIAL2) && defined(SPRACINGF3)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -273,14 +273,12 @@ static void initActiveBoxIds(void)
|
|||
if (feature(FEATURE_SERVO_TILT))
|
||||
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
|
||||
|
||||
bool isFixedWing = mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE;
|
||||
|
||||
#ifdef GPS
|
||||
if (sensors(SENSOR_BARO) || (isFixedWing && feature(FEATURE_GPS))) {
|
||||
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
|
||||
}
|
||||
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (isFixedWing && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
|
||||
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
|
||||
|
|
@ -289,7 +287,7 @@ static void initActiveBoxIds(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
if (isFixedWing) {
|
||||
if (STATE(FIXED_WING)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
|
||||
|
|
@ -300,7 +298,7 @@ static void initActiveBoxIds(void)
|
|||
* FLAPERON mode active only in case of airplane and custom airplane. Activating on
|
||||
* flying wing can cause bad thing
|
||||
*/
|
||||
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
if (STATE(FLAPERON_AVAILABLE)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -587,7 +585,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU16(dst, acc.accADC[i] / scale);
|
||||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
sbufWriteU16(dst, gyro.gyroADC[i]);
|
||||
sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
|
||||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
sbufWriteU16(dst, mag.magADC[i]);
|
||||
|
|
@ -644,11 +642,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
|
||||
case MSP_ALTITUDE:
|
||||
#if defined(NAV)
|
||||
sbufWriteU32(dst, (uint32_t)lrintf(getEstimatedActualPosition(Z)));
|
||||
sbufWriteU16(dst, (uint32_t)lrintf(getEstimatedActualVelocity(Z)));
|
||||
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
|
||||
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
|
||||
#else
|
||||
sbufWriteU32(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
#if defined(BARO)
|
||||
sbufWriteU32(dst, baroGetLatestAltitude());
|
||||
#else
|
||||
sbufWriteU32(dst, 0);
|
||||
#endif
|
||||
break;
|
||||
|
||||
|
|
@ -1148,7 +1151,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
sbufWriteU8(dst, 0); // rangefinder hardware
|
||||
#ifdef SONAR
|
||||
sbufWriteU8(dst, rangefinderConfig()->rangefinder_hardware);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
sbufWriteU8(dst, 0); // optical flow hardware
|
||||
break;
|
||||
|
||||
|
|
@ -1836,6 +1843,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
case MSP_SET_MIXER:
|
||||
mixerConfigMutable()->mixerMode = sbufReadU8(src);
|
||||
mixerUpdateStateFlags(); // Required for correct preset functionality
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
@ -1893,6 +1901,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
sbufReadU8(src); // mixerMode ignored
|
||||
#else
|
||||
mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode
|
||||
mixerUpdateStateFlags(); // Required for correct preset functionality
|
||||
#endif
|
||||
|
||||
featureClearAll();
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclinat
|
|||
|
||||
static imuRuntimeConfig_t imuRuntimeConfig;
|
||||
|
||||
static float gyroScale;
|
||||
static const float gyroScale = (M_PIf / 180.0f); // gyro output scaled to rad per second
|
||||
|
||||
static bool gpsHeadingInitialized = false;
|
||||
|
||||
|
|
@ -118,7 +118,7 @@ void imuUpdateGyroscope(uint32_t gyroUpdateDeltaUs)
|
|||
const float gyroUpdateDelta = gyroUpdateDeltaUs * 1e-6f;
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
imuAccumulatedRate[axis] += gyro.gyroADC[axis] * gyroScale * gyroUpdateDelta;
|
||||
imuAccumulatedRate[axis] += gyro.gyroADCf[axis] * gyroScale * gyroUpdateDelta;
|
||||
}
|
||||
|
||||
imuAccumulatedRateTime += gyroUpdateDelta;
|
||||
|
|
@ -164,7 +164,6 @@ void imuConfigure(void)
|
|||
void imuInit(void)
|
||||
{
|
||||
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
|
||||
gyroScale = gyro.dev.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
imuAccelInBodyFrame.A[axis] = 0;
|
||||
|
|
@ -550,7 +549,7 @@ static void imuUpdateMeasuredRotationRate(void)
|
|||
imuAccumulatedRateTime = 0.0f;
|
||||
#else
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
imuMeasuredRotationBF.A[axis] = gyro.gyroADC[axis] * gyroScale;
|
||||
imuMeasuredRotationBF.A[axis] = gyro.gyroADCf[axis] * gyroScale;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -304,6 +304,27 @@ bool isMixerEnabled(mixerMode_e mixerMode)
|
|||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void mixerUpdateStateFlags(void)
|
||||
{
|
||||
const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
|
||||
|
||||
// set flag that we're on something with wings
|
||||
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||
currentMixerMode == MIXER_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
|
||||
) {
|
||||
ENABLE_STATE(FIXED_WING);
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
}
|
||||
|
||||
if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
ENABLE_STATE(FLAPERON_AVAILABLE);
|
||||
} else {
|
||||
DISABLE_STATE(FLAPERON_AVAILABLE);
|
||||
}
|
||||
}
|
||||
|
||||
void mixerUsePWMIOConfiguration(void)
|
||||
{
|
||||
int i;
|
||||
|
|
|
|||
|
|
@ -123,6 +123,7 @@ extern bool motorLimitReached;
|
|||
void writeAllMotors(int16_t mc);
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
||||
void mixerUsePWMIOConfiguration(void);
|
||||
void mixerUpdateStateFlags(void);
|
||||
void mixerResetDisarmedMotors(void);
|
||||
void mixTable(void);
|
||||
void writeMotors(void);
|
||||
|
|
|
|||
|
|
@ -684,7 +684,7 @@ void pidController(void)
|
|||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
// Step 1: Calculate gyro rates
|
||||
pidState[axis].gyroRate = gyro.gyroADC[axis] * gyro.dev.scale;
|
||||
pidState[axis].gyroRate = gyro.gyroADCf[axis];
|
||||
|
||||
// Step 2: Read target
|
||||
float rateTarget;
|
||||
|
|
|
|||
|
|
@ -199,21 +199,6 @@ int servoDirection(int servoIndex, int inputSource)
|
|||
void servosInit(void)
|
||||
{
|
||||
const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
|
||||
// set flag that we're on something with wings
|
||||
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||
currentMixerMode == MIXER_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
|
||||
) {
|
||||
ENABLE_STATE(FIXED_WING);
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
}
|
||||
|
||||
if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
ENABLE_STATE(FLAPERON_AVAILABLE);
|
||||
} else {
|
||||
DISABLE_STATE(FLAPERON_AVAILABLE);
|
||||
}
|
||||
|
||||
minServoIndex = servoMixers[currentMixerMode].minServoIndex;
|
||||
maxServoIndex = servoMixers[currentMixerMode].maxServoIndex;
|
||||
|
|
|
|||
|
|
@ -269,6 +269,11 @@ int32_t baroCalculateAltitude(void)
|
|||
return baro.BaroAlt;
|
||||
}
|
||||
|
||||
int32_t baroGetLatestAltitude(void)
|
||||
{
|
||||
return baro.BaroAlt;
|
||||
}
|
||||
|
||||
bool baroIsHealthy(void)
|
||||
{
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -52,4 +52,5 @@ void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
|||
uint32_t baroUpdate(void);
|
||||
bool baroIsReady(void);
|
||||
int32_t baroCalculateAltitude(void);
|
||||
int32_t baroGetLatestAltitude(void);
|
||||
bool baroIsHealthy(void);
|
||||
|
|
|
|||
|
|
@ -66,7 +66,8 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
|
|||
.mag_align = ALIGN_DEFAULT,
|
||||
.mag_hardware = MAG_HARDWARE_DEFAULT,
|
||||
.mag_declination = 0,
|
||||
.mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT
|
||||
.mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT,
|
||||
.magCalibrationTimeLimit = 30
|
||||
);
|
||||
|
||||
#ifdef MAG
|
||||
|
|
@ -309,7 +310,7 @@ void compassUpdate(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
if (calStartedAt != 0) {
|
||||
if ((currentTimeUs - calStartedAt) < 30000000) { // 30s: you have 30s to turn the multi in all directions
|
||||
if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
|
||||
LED0_TOGGLE;
|
||||
|
||||
float diffMag = 0;
|
||||
|
|
|
|||
|
|
@ -57,7 +57,8 @@ typedef struct compassConfig_s {
|
|||
sensor_align_e mag_align; // mag alignment
|
||||
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
|
||||
flightDynamicsTrims_t magZero;
|
||||
uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
|
||||
uint8_t mag_hold_rate_limit; // Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
|
||||
uint8_t magCalibrationTimeLimit; // Time for compass calibration (seconds)
|
||||
} compassConfig_t;
|
||||
|
||||
PG_DECLARE(compassConfig_t, compassConfig);
|
||||
|
|
|
|||
|
|
@ -69,9 +69,14 @@
|
|||
|
||||
gyro_t gyro; // gyro sensor object
|
||||
|
||||
STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
|
||||
typedef struct gyroCalibration_s {
|
||||
int32_t g[XYZ_AXIS_COUNT];
|
||||
stdev_t var[XYZ_AXIS_COUNT];
|
||||
uint16_t calibratingG;
|
||||
} gyroCalibration_t;
|
||||
|
||||
static uint16_t calibratingG = 0;
|
||||
STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration;
|
||||
static int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
|
||||
static filterApplyFnPtr softLpfFilterApplyFn;
|
||||
static void *softLpfFilter[XYZ_AXIS_COUNT];
|
||||
|
|
@ -115,12 +120,8 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig)
|
||||
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
|
||||
{
|
||||
dev->mpuIntExtiConfig = extiConfig;
|
||||
|
||||
gyroSensor_e gyroHardware = GYRO_AUTODETECT;
|
||||
|
||||
dev->gyroAlign = ALIGN_DEFAULT;
|
||||
|
||||
switch(gyroHardware) {
|
||||
|
|
@ -196,8 +197,8 @@ STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfi
|
|||
#ifdef GYRO_MPU6500_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU6500_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
|
|
@ -209,20 +210,18 @@ STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfi
|
|||
}
|
||||
#endif
|
||||
; // fallthrough
|
||||
default:
|
||||
case GYRO_NONE:
|
||||
gyroHardware = GYRO_NONE;
|
||||
}
|
||||
|
||||
addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0);
|
||||
|
||||
if (gyroHardware == GYRO_NONE) {
|
||||
return false;
|
||||
if (gyroHardware != GYRO_NONE) {
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
||||
sensorsSet(SENSOR_GYRO);
|
||||
}
|
||||
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
||||
sensorsSet(SENSOR_GYRO);
|
||||
|
||||
return true;
|
||||
return gyroHardware;
|
||||
}
|
||||
|
||||
bool gyroInit(void)
|
||||
|
|
@ -235,8 +234,8 @@ bool gyroInit(void)
|
|||
#else
|
||||
const extiConfig_t *extiConfig = NULL;
|
||||
#endif
|
||||
|
||||
if (!gyroDetect(&gyro.dev, extiConfig)) {
|
||||
gyro.dev.mpuIntExtiConfig = extiConfig;
|
||||
if (gyroDetect(&gyro.dev, GYRO_AUTODETECT) == GYRO_NONE) {
|
||||
return false;
|
||||
}
|
||||
// After refactoring this function is always called after gyro sampling rate is known, so
|
||||
|
|
@ -298,83 +297,90 @@ void gyroInitFilters(void)
|
|||
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
{
|
||||
calibratingG = calibrationCyclesRequired;
|
||||
gyroCalibration.calibratingG = calibrationCyclesRequired;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration)
|
||||
{
|
||||
return gyroCalibration->calibratingG == 0;
|
||||
}
|
||||
|
||||
bool gyroIsCalibrationComplete(void)
|
||||
{
|
||||
return calibratingG == 0;
|
||||
return gyroCalibration.calibratingG == 0;
|
||||
}
|
||||
|
||||
static bool isOnFinalGyroCalibrationCycle(void)
|
||||
static bool isOnFinalGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
|
||||
{
|
||||
return calibratingG == 1;
|
||||
return gyroCalibration->calibratingG == 1;
|
||||
}
|
||||
|
||||
static bool isOnFirstGyroCalibrationCycle(void)
|
||||
static bool isOnFirstGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
|
||||
{
|
||||
return calibratingG == CALIBRATING_GYRO_CYCLES;
|
||||
return gyroCalibration->calibratingG == CALIBRATING_GYRO_CYCLES;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
|
||||
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
|
||||
{
|
||||
static int32_t g[3];
|
||||
static stdev_t var[3];
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
|
||||
// Reset g[axis] at start of calibration
|
||||
if (isOnFirstGyroCalibrationCycle()) {
|
||||
g[axis] = 0;
|
||||
devClear(&var[axis]);
|
||||
if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
|
||||
gyroCalibration->g[axis] = 0;
|
||||
devClear(&gyroCalibration->var[axis]);
|
||||
}
|
||||
|
||||
// Sum up CALIBRATING_GYRO_CYCLES readings
|
||||
g[axis] += gyro.gyroADC[axis];
|
||||
devPush(&var[axis], gyro.gyroADC[axis]);
|
||||
gyroCalibration->g[axis] += dev->gyroADCRaw[axis];
|
||||
devPush(&gyroCalibration->var[axis], dev->gyroADCRaw[axis]);
|
||||
|
||||
// Reset global variables to prevent other code from using un-calibrated data
|
||||
gyro.gyroADC[axis] = 0;
|
||||
gyroZero[axis] = 0;
|
||||
// gyroZero is set to zero until calibration complete
|
||||
dev->gyroZero[axis] = 0;
|
||||
|
||||
if (isOnFinalGyroCalibrationCycle()) {
|
||||
float dev = devStandardDeviation(&var[axis]);
|
||||
// check deviation and startover in case the model was moved
|
||||
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
|
||||
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
|
||||
const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
|
||||
// check deviation and start over if the model was moved
|
||||
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
|
||||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||||
return;
|
||||
}
|
||||
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
|
||||
// calibration complete, so set the gyro zero values
|
||||
dev->gyroZero[axis] = (gyroCalibration->g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
|
||||
}
|
||||
}
|
||||
|
||||
if (isOnFinalGyroCalibrationCycle()) {
|
||||
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
|
||||
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
|
||||
beeper(BEEPER_GYRO_CALIBRATED);
|
||||
}
|
||||
calibratingG--;
|
||||
|
||||
gyroCalibration->calibratingG--;
|
||||
}
|
||||
|
||||
void gyroUpdate(void)
|
||||
{
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
if (!gyro.dev.read(&gyro.dev)) {
|
||||
if (gyro.dev.read(&gyro.dev)) {
|
||||
if (isCalibrationComplete(&gyroCalibration)) {
|
||||
// Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
|
||||
gyroADC[X] = (int32_t)gyro.dev.gyroADCRaw[X] - (int32_t)gyro.dev.gyroZero[X];
|
||||
gyroADC[Y] = (int32_t)gyro.dev.gyroADCRaw[Y] - (int32_t)gyro.dev.gyroZero[Y];
|
||||
gyroADC[Z] = (int32_t)gyro.dev.gyroADCRaw[Z] - (int32_t)gyro.dev.gyroZero[Z];
|
||||
alignSensors(gyroADC, gyro.dev.gyroAlign);
|
||||
} else {
|
||||
performGyroCalibration(&gyro.dev, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
|
||||
// Reset gyro values to zero to prevent other code from using uncalibrated data
|
||||
gyro.gyroADCf[X] = 0.0f;
|
||||
gyro.gyroADCf[Y] = 0.0f;
|
||||
gyro.gyroADCf[Z] = 0.0f;
|
||||
// still calibrating, so no need to further process gyro data
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// no gyro reading to process
|
||||
return;
|
||||
}
|
||||
|
||||
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
|
||||
gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
||||
gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
|
||||
if (!gyroIsCalibrationComplete()) {
|
||||
performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
|
||||
}
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyro.gyroADC[axis] -= gyroZero[axis];
|
||||
float gyroADCf = (float)gyro.gyroADC[axis];
|
||||
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
|
||||
|
||||
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
|
||||
|
||||
|
|
@ -388,8 +394,6 @@ void gyroUpdate(void)
|
|||
#ifdef USE_GYRO_NOTCH_2
|
||||
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
|
||||
#endif
|
||||
gyro.gyroADC[axis] = lrintf(gyroADCf);
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
|
||||
alignSensors(gyro.gyroADC, gyro.dev.gyroAlign);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ typedef enum {
|
|||
typedef struct gyro_s {
|
||||
gyroDev_t dev;
|
||||
uint32_t targetLooptime;
|
||||
int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
float gyroADCf[XYZ_AXIS_COUNT];
|
||||
} gyro_t;
|
||||
|
||||
extern gyro_t gyro;
|
||||
|
|
|
|||
|
|
@ -67,8 +67,7 @@ bool sensorsAutodetect(void)
|
|||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
const rangefinderType_e rangefinderType = rangefinderDetect();
|
||||
rangefinderInit(rangefinderType);
|
||||
rangefinderInit();
|
||||
#endif
|
||||
|
||||
if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) {
|
||||
|
|
|
|||
|
|
@ -22,14 +22,14 @@
|
|||
|
||||
#include <platform.h>
|
||||
|
||||
#ifdef SONAR
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/logging.h"
|
||||
|
|
@ -44,139 +44,96 @@
|
|||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
// Sonar measurements are in cm, a value of RANGEFINDER_OUT_OF_RANGE indicates sonar is not in range.
|
||||
// Inclination is adjusted by imu
|
||||
rangefinder_t rangefinder;
|
||||
|
||||
extern sonarHcsr04Hardware_t sonarHcsr04Hardware;
|
||||
#ifdef SONAR
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
|
||||
|
||||
static int16_t rangefinderMaxRangeCm;
|
||||
static int16_t rangefinderMaxAltWithTiltCm;
|
||||
static int16_t rangefinderCfAltCm; // Complimentary Filter altitude
|
||||
STATIC_UNIT_TESTED int16_t rangefinderMaxTiltDeciDegrees;
|
||||
static rangefinderFunctionPointers_t rangefinderFunctionPointers;
|
||||
static float rangefinderMaxTiltCos;
|
||||
PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
|
||||
.rangefinder_hardware = RANGEFINDER_NONE,
|
||||
);
|
||||
|
||||
static int32_t calculatedAltitude;
|
||||
const rangefinderHardwarePins_t * sonarGetHardwarePins(void)
|
||||
{
|
||||
static rangefinderHardwarePins_t rangefinderHardwarePins;
|
||||
|
||||
#if defined(SONAR_PWM_TRIGGER_PIN)
|
||||
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins
|
||||
if (feature(FEATURE_SOFTSERIAL)
|
||||
|| feature(FEATURE_RX_PARALLEL_PWM )
|
||||
|| (feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC)) {
|
||||
rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
|
||||
rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
|
||||
} else {
|
||||
rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
|
||||
rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN);
|
||||
}
|
||||
#elif defined(SONAR_TRIGGER_PIN)
|
||||
rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
|
||||
rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN);
|
||||
#else
|
||||
#error Sonar not defined for target
|
||||
#endif
|
||||
return &rangefinderHardwarePins;
|
||||
}
|
||||
|
||||
/*
|
||||
* Detect which rangefinder is present
|
||||
*/
|
||||
rangefinderType_e rangefinderDetect(void)
|
||||
static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwareToUse)
|
||||
{
|
||||
rangefinderType_e rangefinderType = RANGEFINDER_NONE;
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
// the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
|
||||
// since there is no way to detect it
|
||||
rangefinderType = RANGEFINDER_HCSR04;
|
||||
}
|
||||
rangefinderType_e rangefinderHardware = RANGEFINDER_NONE;
|
||||
requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse;
|
||||
|
||||
switch (rangefinderHardwareToUse) {
|
||||
case RANGEFINDER_HCSR04:
|
||||
{
|
||||
const rangefinderHardwarePins_t *sonarHardwarePins = sonarGetHardwarePins();
|
||||
if (hcsr04Detect(dev, sonarHardwarePins)) { // FIXME: Do actual detection if HC-SR04 is plugged in
|
||||
rangefinderHardware = RANGEFINDER_HCSR04;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case RANGEFINDER_SRF10:
|
||||
#ifdef USE_SONAR_SRF10
|
||||
if (srf10_detect()) {
|
||||
// if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04
|
||||
rangefinderType = RANGEFINDER_SRF10;
|
||||
}
|
||||
if (srf10Detect(dev)) {
|
||||
rangefinderHardware = RANGEFINDER_SRF10;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderType, 0, 0, 0);
|
||||
|
||||
requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; // FIXME: Make rangefinder type selectable from CLI
|
||||
detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType;
|
||||
|
||||
if (rangefinderType != RANGEFINDER_NONE) {
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
case RANGEFINDER_NONE:
|
||||
rangefinderHardware = RANGEFINDER_NONE;
|
||||
break;
|
||||
}
|
||||
|
||||
return rangefinderType;
|
||||
addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderHardware, 0, 0, 0);
|
||||
|
||||
if (rangefinderHardware == RANGEFINDER_NONE) {
|
||||
sensorsClear(SENSOR_SONAR);
|
||||
return false;
|
||||
}
|
||||
|
||||
detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
return true;
|
||||
}
|
||||
|
||||
static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(currentSensor_e currentSensor)
|
||||
bool rangefinderInit(void)
|
||||
{
|
||||
#if defined(SONAR_PWM_TRIGGER_PIN)
|
||||
#endif
|
||||
#if !defined(UNIT_TEST)
|
||||
#endif
|
||||
#if defined(UNIT_TEST)
|
||||
UNUSED(currentSensor);
|
||||
return 0;
|
||||
#elif defined(SONAR_PWM_TRIGGER_PIN)
|
||||
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins
|
||||
if (feature(FEATURE_SOFTSERIAL)
|
||||
|| feature(FEATURE_RX_PARALLEL_PWM )
|
||||
|| (feature(FEATURE_CURRENT_METER) && currentSensor == CURRENT_SENSOR_ADC)) {
|
||||
sonarHcsr04Hardware.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
|
||||
sonarHcsr04Hardware.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
|
||||
} else {
|
||||
sonarHcsr04Hardware.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
|
||||
sonarHcsr04Hardware.echoTag = IO_TAG(SONAR_ECHO_PIN);
|
||||
if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
|
||||
return false;
|
||||
}
|
||||
#elif defined(SONAR_TRIGGER_PIN)
|
||||
UNUSED(currentSensor);
|
||||
sonarHcsr04Hardware.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
|
||||
sonarHcsr04Hardware.echoTag = IO_TAG(SONAR_ECHO_PIN);
|
||||
#else
|
||||
#error Sonar not defined for target
|
||||
#endif
|
||||
return &sonarHcsr04Hardware;
|
||||
|
||||
rangefinder.dev.init();
|
||||
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED void rangefinderSetFunctionPointers(rangefinderType_e rangefinderType)
|
||||
{
|
||||
|
||||
switch (rangefinderType) {
|
||||
default:
|
||||
case RANGEFINDER_NONE:
|
||||
break;
|
||||
case RANGEFINDER_HCSR04:
|
||||
rangefinderFunctionPointers.init = hcsr04_init;
|
||||
rangefinderFunctionPointers.update = hcsr04_start_reading;
|
||||
rangefinderFunctionPointers.read = hcsr04_get_distance;
|
||||
break;
|
||||
#ifdef USE_SONAR_SRF10
|
||||
case RANGEFINDER_SRF10:
|
||||
rangefinderFunctionPointers.init = srf10_init;
|
||||
rangefinderFunctionPointers.update = srf10_start_reading;
|
||||
rangefinderFunctionPointers.read = srf10_get_distance;
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Get the HCSR04 sonar hardware configuration.
|
||||
* NOTE: sonarInit() must be subsequently called before using any of the sonar functions.
|
||||
*/
|
||||
const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor)
|
||||
{
|
||||
// Return the configuration for the HC-SR04 hardware.
|
||||
// Unfortunately the I2C bus is not initialised at this point
|
||||
// so cannot detect if another sonar device is present
|
||||
return sonarGetHardwareConfigurationForHCSR04(currentSensor);
|
||||
}
|
||||
|
||||
void rangefinderInit(rangefinderType_e rangefinderType)
|
||||
{
|
||||
calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
|
||||
if (rangefinderType == RANGEFINDER_HCSR04) {
|
||||
hcsr04_set_sonar_hardware();
|
||||
}
|
||||
#ifndef UNIT_TEST
|
||||
rangefinderSetFunctionPointers(rangefinderType);
|
||||
#endif
|
||||
|
||||
rangefinder_t rangefinder;
|
||||
if (rangefinderType == RANGEFINDER_NONE) {
|
||||
memset(&rangefinder, 0, sizeof(rangefinder));
|
||||
} else {
|
||||
rangefinderFunctionPointers.init(&rangefinder);
|
||||
}
|
||||
rangefinderMaxRangeCm = rangefinder.maxRangeCm;
|
||||
rangefinderCfAltCm = rangefinderMaxRangeCm / 2;
|
||||
rangefinderMaxTiltDeciDegrees = rangefinder.detectionConeExtendedDeciDegrees / 2;
|
||||
rangefinderMaxTiltCos = cos_approx(rangefinderMaxTiltDeciDegrees / 10.0f * RAD);
|
||||
rangefinderMaxAltWithTiltCm = rangefinderMaxRangeCm * rangefinderMaxTiltCos;
|
||||
}
|
||||
|
||||
|
||||
static int32_t applyMedianFilter(int32_t newReading)
|
||||
{
|
||||
#define DISTANCE_SAMPLES_MEDIAN 5
|
||||
|
|
@ -200,8 +157,8 @@ static int32_t applyMedianFilter(int32_t newReading)
|
|||
*/
|
||||
void rangefinderUpdate(void)
|
||||
{
|
||||
if (rangefinderFunctionPointers.update) {
|
||||
rangefinderFunctionPointers.update();
|
||||
if (rangefinder.dev.update) {
|
||||
rangefinder.dev.update();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -210,11 +167,20 @@ void rangefinderUpdate(void)
|
|||
*/
|
||||
int32_t rangefinderRead(void)
|
||||
{
|
||||
if (rangefinderFunctionPointers.read) {
|
||||
const int32_t distance = rangefinderFunctionPointers.read();
|
||||
return applyMedianFilter(distance);
|
||||
if (rangefinder.dev.read) {
|
||||
const int32_t distance = rangefinder.dev.read();
|
||||
if (distance >= 0) {
|
||||
rangefinder.rawAltitude = applyMedianFilter(distance);
|
||||
}
|
||||
else {
|
||||
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
else {
|
||||
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
return rangefinder.rawAltitude;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -226,12 +192,12 @@ int32_t rangefinderRead(void)
|
|||
int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle)
|
||||
{
|
||||
// calculate sonar altitude only if the ground is in the sonar cone
|
||||
if (cosTiltAngle < rangefinderMaxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) {
|
||||
calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
if (cosTiltAngle < rangefinder.maxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) {
|
||||
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
} else {
|
||||
calculatedAltitude = rangefinderDistance * cosTiltAngle;
|
||||
rangefinder.calculatedAltitude = rangefinderDistance * cosTiltAngle;
|
||||
}
|
||||
return calculatedAltitude;
|
||||
return rangefinder.calculatedAltitude;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -240,7 +206,7 @@ int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltA
|
|||
*/
|
||||
int32_t rangefinderGetLatestAltitude(void)
|
||||
{
|
||||
return calculatedAltitude;
|
||||
return rangefinder.calculatedAltitude;
|
||||
}
|
||||
|
||||
bool rangefinderIsHealthy(void)
|
||||
|
|
|
|||
|
|
@ -18,6 +18,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "config/parameter_group.h"
|
||||
#include "drivers/rangefinder.h"
|
||||
|
||||
typedef enum {
|
||||
RANGEFINDER_NONE = 0,
|
||||
|
|
@ -25,22 +27,30 @@ typedef enum {
|
|||
RANGEFINDER_SRF10 = 2,
|
||||
} rangefinderType_e;
|
||||
|
||||
struct rangefinder_s;
|
||||
typedef void (*rangefinderInitFunctionPtr)(struct rangefinder_s *rangefinderRange);
|
||||
typedef void (*rangefinderUpdateFunctionPtr)(void);
|
||||
typedef int32_t (*rangefinderReadFunctionPtr)(void);
|
||||
typedef struct rangefinderConfig_s {
|
||||
uint8_t rangefinder_hardware;
|
||||
} rangefinderConfig_t;
|
||||
|
||||
PG_DECLARE(rangefinderConfig_t, rangefinderConfig);
|
||||
|
||||
typedef struct rangefinder_s {
|
||||
rangefinderDev_t dev;
|
||||
float maxTiltCos;
|
||||
int32_t rawAltitude;
|
||||
int32_t calculatedAltitude;
|
||||
} rangefinder_t;
|
||||
|
||||
extern rangefinder_t rangefinder;
|
||||
|
||||
const rangefinderHardwarePins_t * sonarGetHardwarePins(void);
|
||||
|
||||
bool rangefinderInit(void);
|
||||
|
||||
|
||||
typedef struct rangefinderFunctionPointers_s {
|
||||
rangefinderInitFunctionPtr init;
|
||||
rangefinderUpdateFunctionPtr update;
|
||||
rangefinderReadFunctionPtr read;
|
||||
} rangefinderFunctionPointers_t;
|
||||
|
||||
rangefinderType_e rangefinderDetect(void);
|
||||
int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle);
|
||||
int32_t rangefinderGetLatestAltitude(void);
|
||||
rangefinderType_e rangefinderDetect(void);
|
||||
void rangefinderInit(rangefinderType_e rangefinderType);
|
||||
|
||||
void rangefinderUpdate(void);
|
||||
int32_t rangefinderRead(void);
|
||||
bool rangefinderIsHealthy(void);
|
||||
|
|
|
|||
|
|
@ -1,52 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/rangefinder.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
typedef enum {
|
||||
SONAR_NONE = 0,
|
||||
SONAR_HCSR04,
|
||||
SONAR_SRF10
|
||||
} sonarHardwareType_e;
|
||||
|
||||
typedef enum {
|
||||
SONAR_HCSR04_PINS_RC,
|
||||
SONAR_HCSR04_PINS_PWM,
|
||||
} sonarHCSR04Pins_e;
|
||||
|
||||
struct sonarRange_s;
|
||||
typedef void (*sonarInitFunctionPtr)(struct sonarRange_s *sonarRange);
|
||||
typedef void (*sonarUpdateFunctionPtr)(void);
|
||||
typedef int32_t (*sonarReadFunctionPtr)(void);
|
||||
|
||||
typedef struct sonarFunctionPointers_s {
|
||||
sonarInitFunctionPtr init;
|
||||
sonarUpdateFunctionPtr update;
|
||||
sonarReadFunctionPtr read;
|
||||
} sonarFunctionPointers_t;
|
||||
|
||||
const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor);
|
||||
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);
|
||||
int32_t sonarGetLatestAltitude(void);
|
||||
void sonarInit(void);
|
||||
void sonarUpdate(void);
|
||||
int32_t sonarRead(void);
|
||||
|
||||
|
|
@ -29,6 +29,7 @@
|
|||
#include "drivers/flash_m25p16.h"
|
||||
#include "hardware_revision.h"
|
||||
|
||||
#if 0
|
||||
static const char * const hardwareRevisionNames[] = {
|
||||
"Unknown",
|
||||
"BlueJay rev1",
|
||||
|
|
@ -36,6 +37,7 @@ static const char * const hardwareRevisionNames[] = {
|
|||
"BlueJay rev3",
|
||||
"BlueJay rev3a"
|
||||
};
|
||||
#endif
|
||||
|
||||
uint8_t hardwareRevision = UNKNOWN;
|
||||
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ extern "C" {
|
|||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
#include "drivers/accgyro_fake.h"
|
||||
#include "drivers/logging_codes.h"
|
||||
|
|
@ -36,9 +37,16 @@ extern "C" {
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig);
|
||||
STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold);
|
||||
extern int32_t gyroZero[XYZ_AXIS_COUNT];
|
||||
typedef struct gyroCalibration_s {
|
||||
int32_t g[XYZ_AXIS_COUNT];
|
||||
stdev_t var[XYZ_AXIS_COUNT];
|
||||
uint16_t calibratingG;
|
||||
} gyroCalibration_t;
|
||||
extern gyroCalibration_t gyroCalibration;
|
||||
|
||||
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
|
||||
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold);
|
||||
STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration);
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
|
@ -46,14 +54,15 @@ extern "C" {
|
|||
|
||||
TEST(SensorGyro, Detect)
|
||||
{
|
||||
const bool detected = gyroDetect(&gyro.dev, NULL);
|
||||
EXPECT_EQ(true, detected);
|
||||
const gyroSensor_e detected = gyroDetect(&gyro.dev, GYRO_AUTODETECT);
|
||||
EXPECT_EQ(GYRO_FAKE, detected);
|
||||
EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
|
||||
}
|
||||
|
||||
TEST(SensorGyro, Init)
|
||||
{
|
||||
gyroInit();
|
||||
const bool initialised = gyroInit();
|
||||
EXPECT_EQ(true, initialised);
|
||||
EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
|
||||
}
|
||||
|
||||
|
|
@ -75,30 +84,54 @@ TEST(SensorGyro, Calibrate)
|
|||
fakeGyroSet(5, 6, 7);
|
||||
const bool read = gyro.dev.read(&gyro.dev);
|
||||
EXPECT_EQ(true, read);
|
||||
gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
||||
gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
EXPECT_EQ(5, gyro.gyroADC[X]);
|
||||
EXPECT_EQ(6, gyro.gyroADC[Y]);
|
||||
EXPECT_EQ(7, gyro.gyroADC[Z]);
|
||||
EXPECT_EQ(5, gyro.dev.gyroADCRaw[X]);
|
||||
EXPECT_EQ(6, gyro.dev.gyroADCRaw[Y]);
|
||||
EXPECT_EQ(7, gyro.dev.gyroADCRaw[Z]);
|
||||
static const int gyroMovementCalibrationThreshold = 32;
|
||||
gyroZero[X] = 8;
|
||||
gyroZero[Y] = 9;
|
||||
gyroZero[Z] = 10;
|
||||
performGyroCalibration(gyroMovementCalibrationThreshold);
|
||||
EXPECT_EQ(0, gyroZero[X]);
|
||||
EXPECT_EQ(0, gyroZero[Y]);
|
||||
EXPECT_EQ(0, gyroZero[Z]);
|
||||
EXPECT_EQ(false, gyroIsCalibrationComplete());
|
||||
while (!gyroIsCalibrationComplete()) {
|
||||
performGyroCalibration(gyroMovementCalibrationThreshold);
|
||||
gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
||||
gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
gyro.dev.gyroZero[X] = 8;
|
||||
gyro.dev.gyroZero[Y] = 9;
|
||||
gyro.dev.gyroZero[Z] = 10;
|
||||
performGyroCalibration(&gyro.dev, &gyroCalibration, gyroMovementCalibrationThreshold);
|
||||
EXPECT_EQ(0, gyro.dev.gyroZero[X]);
|
||||
EXPECT_EQ(0, gyro.dev.gyroZero[Y]);
|
||||
EXPECT_EQ(0, gyro.dev.gyroZero[Z]);
|
||||
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
|
||||
while (!isCalibrationComplete(&gyroCalibration)) {
|
||||
performGyroCalibration(&gyro.dev, &gyroCalibration, gyroMovementCalibrationThreshold);
|
||||
}
|
||||
EXPECT_EQ(5, gyroZero[X]);
|
||||
EXPECT_EQ(6, gyroZero[Y]);
|
||||
EXPECT_EQ(7, gyroZero[Z]);
|
||||
EXPECT_EQ(5, gyro.dev.gyroZero[X]);
|
||||
EXPECT_EQ(6, gyro.dev.gyroZero[Y]);
|
||||
EXPECT_EQ(7, gyro.dev.gyroZero[Z]);
|
||||
}
|
||||
|
||||
TEST(SensorGyro, Update)
|
||||
{
|
||||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||||
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
|
||||
gyroInit();
|
||||
fakeGyroSet(5, 6, 7);
|
||||
gyroUpdate();
|
||||
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
|
||||
while (!isCalibrationComplete(&gyroCalibration)) {
|
||||
gyroUpdate();
|
||||
}
|
||||
EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration));
|
||||
EXPECT_EQ(5, gyro.dev.gyroZero[X]);
|
||||
EXPECT_EQ(6, gyro.dev.gyroZero[Y]);
|
||||
EXPECT_EQ(7, gyro.dev.gyroZero[Z]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
||||
gyroUpdate();
|
||||
// expect zero values since gyro is calibrated
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
||||
fakeGyroSet(15, 26, 97);
|
||||
gyroUpdate();
|
||||
EXPECT_FLOAT_EQ(10 * gyro.dev.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
|
||||
EXPECT_FLOAT_EQ(20 * gyro.dev.scale, gyro.gyroADCf[Y]);
|
||||
EXPECT_FLOAT_EQ(90 * gyro.dev.scale, gyro.gyroADCf[Z]);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue