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

Tidied up sonar targets

This commit is contained in:
Martin Budden 2016-06-23 11:04:38 +01:00
parent 23e2fd55c2
commit 3ca385d92e
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