rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #573 from martinbudden/bf_sonar_tidy2 

Tidied up sonar targets - for review
This commit is contained in:
J Blackman 2016-06-24 08:38:40 +10:00 committed by GitHub
parent 23e2fd55c2 3ca385d92e
commit d4037e05e4
13 changed files with 57 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/drivers/pwm_mapping.c
View File

@ -189,10 +189,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
#endif
#ifdef SONAR
if (init->sonarConfig &&
if (init->useSonar &&
(
timerHardwarePtr->tag == init->sonarConfig->triggerTag ||
timerHardwarePtr->tag == init->sonarConfig->echoTag
timerHardwarePtr->tag == init->sonarConfig.triggerTag ||
timerHardwarePtr->tag == init->sonarConfig.echoTag
)) {
continue;
}

2
src/main/drivers/pwm_mapping.h
View File

@ -77,7 +77,7 @@ typedef struct drv_pwm_config_s {
uint16_t motorPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
// some higher value (used by 3d mode), or 0, for brushed pwm drivers.
sonarIOConfig_t *sonarConfig;
sonarIOConfig_t sonarConfig;
} drv_pwm_config_t;
enum {

9
src/main/drivers/sonar_hcsr04.c
View File

@ -39,7 +39,7 @@
#if defined(SONAR)
STATIC_UNIT_TESTED volatile int32_t measurement = -1;
static uint32_t lastMeasurementAt;
static sonarHardware_t const *sonarHardware;
sonarHardware_t sonarHardwareHCSR04;
extiCallbackRec_t hcsr04_extiCallbackRec;
@ -63,9 +63,8 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb)
}
}
void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange)
void hcsr04_init(sonarRange_t *sonarRange)
{
sonarHardware = initialSonarHardware;
sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
@ -83,12 +82,12 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sona
#endif
// trigger pin
triggerIO = IOGetByTag(sonarHardware->triggerTag);
triggerIO = IOGetByTag(sonarHardwareHCSR04.triggerTag);
IOInit(triggerIO, OWNER_SONAR, RESOURCE_INPUT);
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
// echo pin
echoIO = IOGetByTag(sonarHardware->echoTag);
echoIO = IOGetByTag(sonarHardwareHCSR04.echoTag);
IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT);
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);

2
src/main/drivers/sonar_hcsr04.h
View File

@ -38,6 +38,6 @@ typedef struct sonarRange_s {
#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
void hcsr04_init(const sonarHardware_t *sonarHardware, sonarRange_t *sonarRange);
void hcsr04_init(sonarRange_t *sonarRange);
void hcsr04_start_reading(void);
int32_t hcsr04_get_distance(void);

22
src/main/main.c
View File

@ -137,8 +137,7 @@ void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
void spektrumBind(rxConfig_t *rxConfig);
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
void sonarInit(const sonarHardware_t *sonarHardware);
const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor);
void osdInit(void);
typedef enum {
@ -264,15 +263,13 @@ void init(void)
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarIOConfig_t sonarConfig = {
.triggerTag = sonarHardware->triggerTag,
.echoTag = sonarHardware->echoTag
};
pwm_params.sonarConfig = &sonarConfig;
const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType);
if (sonarHardware) {
pwm_params.useSonar = true;
pwm_params.sonarConfig.triggerTag = sonarHardware->triggerTag;
pwm_params.sonarConfig.echoTag = sonarHardware->echoTag;
}
}
#endif
@ -302,9 +299,6 @@ void init(void)
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
@ -548,7 +542,7 @@ void init(void)
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
sonarInit();
}
#endif

47
src/main/sensors/sonar.c
View File

@ -22,6 +22,8 @@
#include "platform.h"
#include "build_config.h"
#ifdef SONAR
#include "common/maths.h"
#include "common/axis.h"
@ -36,10 +38,9 @@
// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
// Inclination is adjusted by imu
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
#ifdef SONAR
extern sonarHardware_t sonarHardwareHCSR04;
int16_t sonarMaxRangeCm;
int16_t sonarMaxAltWithTiltCm;
int16_t sonarCfAltCm; // Complimentary Filter altitude
@ -48,34 +49,38 @@ float sonarMaxTiltCos;
static int32_t calculatedAltitude;
#ifdef SONAR_CUSTOM_CONFIG
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig);
#endif
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor)
{
#if defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.triggerTag = IO_TAG(SONAR_TRIGGER_PIN),
.echoTag = IO_TAG(SONAR_ECHO_PIN),
};
return &sonarHardware;
#elif defined(SONAR_CUSTOM_CONFIG)
return sonarGetTargetHardwareConfiguration(batteryConfig);
#if defined(SONAR_TRIGGER_PIN_PWM) && defined(SONAR_ECHO_PIN_PWM)
// 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)) {
sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
} else {
sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN);
}
return &sonarHardwareHCSR04;
#elif defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN)
UNUSED(currentSensor);
sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN);
return &sonarHardwareHCSR04;
#elif defined(UNIT_TEST)
UNUSED(batteryConfig);
return 0;
UNUSED(currentSensor);
return NULL;
#else
#error Sonar not defined for target
#endif
}
void sonarInit(const sonarHardware_t *sonarHardware)
void sonarInit(void)
{
sonarRange_t sonarRange;
hcsr04_init(sonarHardware, &sonarRange);
hcsr04_init(&sonarRange);
sensorsSet(SENSOR_SONAR);
sonarMaxRangeCm = sonarRange.maxRangeCm;
sonarCfAltCm = sonarMaxRangeCm / 2;

1
src/main/sensors/sonar.h
View File

@ -23,6 +23,7 @@ extern int16_t sonarMaxRangeCm;
extern int16_t sonarCfAltCm;
extern int16_t sonarMaxAltWithTiltCm;
void sonarInit(void);
void sonarUpdate(void);
int32_t sonarRead(void);
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);

28
src/main/target/EUSTM32F103RC/sonar_config.c
View File

@ -1,28 +0,0 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
static const sonarHardware_t const sonarPWM56 = {
.triggerTag = IO_TAG(PB8),
.echoTag = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.triggerTag = IO_TAG(PB0),
.echoTag = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
}

5
src/main/target/EUSTM32F103RC/target.h
View File

@ -69,7 +69,10 @@
#define MAG_AK8975_ALIGN CW180_DEG_FLIP
#define SONAR
#define SONAR_CUSTOM_CONFIG
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN_PWM PB8
#define SONAR_ECHO_PIN_PWM PB9
#define DISPLAY

28
src/main/target/NAZE/sonar_config.c
View File

@ -1,28 +0,0 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
static const sonarHardware_t const sonarPWM56 = {
.triggerTag = IO_TAG(PB8),
.echoTag = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.triggerTag = IO_TAG(PB0),
.echoTag = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
}

6
src/main/target/NAZE/target.h
View File

@ -111,7 +111,11 @@
#define MAG_HMC5883_ALIGN CW180_DEG
#define SONAR
#define SONAR_CUSTOM_CONFIG
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN_PWM PB8
#define SONAR_ECHO_PIN_PWM PB9
#define DISPLAY
#define USE_USART1

28
src/main/target/PORT103R/sonar_config.c
View File

@ -1,28 +0,0 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
static const sonarHardware_t const sonarPWM56 = {
.triggerTag = IO_TAG(PB8),
.echoTag = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.triggerTag = IO_TAG(PB0),
.echoTag = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
}

5
src/main/target/PORT103R/target.h
View File

@ -87,7 +87,10 @@
#define USE_FLASH_M25P16
#define SONAR
#define SONAR_CUSTOM_CONFIG
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN_PWM PB8
#define SONAR_ECHO_PIN_PWM PB9
#define DISPLAY