Merge branch 'master' into dev
This commit is contained in:
commit
4658ff5b3c
19 changed files with 209 additions and 70 deletions
1
Makefile
1
Makefile
|
@ -444,6 +444,7 @@ CC3D_SRC = \
|
|||
drivers/serial_softserial.c \
|
||||
drivers/serial_uart.c \
|
||||
drivers/serial_uart_stm32f10x.c \
|
||||
drivers/sonar_hcsr04.c \
|
||||
drivers/sound_beeper_stm32f10x.c \
|
||||
drivers/system_stm32f10x.c \
|
||||
drivers/timer.c \
|
||||
|
|
|
@ -203,9 +203,9 @@ flight problems like vibration or PID setting issues.
|
|||
|
||||
The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm.
|
||||
|
||||
If your craft has a buzzer attached, a short beep will be played when you arm and recording begins. You can later use
|
||||
this beep to synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line
|
||||
in the flight data log, which you can sync against the beep in your recorded audio track).
|
||||
If your craft has a buzzer attached, you can use Cleanflight's arming beep to synchronize your Blackbox log with your
|
||||
flight video. Cleanflight's arming beep is a "long, short" pattern. The beginning of the first long beep will be shown
|
||||
as a blue line in the flight data log, which you can sync against your recorded audio track.
|
||||
|
||||
You should wait a few seconds after disarming your craft to allow the Blackbox to finish saving its data.
|
||||
|
||||
|
@ -224,8 +224,9 @@ minutes.
|
|||

|
||||
|
||||
After downloading the log, be sure to erase the chip to make it ready for reuse by clicking the "erase flash" button.
|
||||
If you try to start recording a new flight when the dataflash is already full, the Blackbox will not make its regular
|
||||
arming beep and nothing will be recorded.
|
||||
|
||||
If you try to start recording a new flight when the dataflash is already full, Blackbox logging will be disabled and
|
||||
nothing will be recorded.
|
||||
|
||||
## Converting logs to CSV or PNG
|
||||
After your flights, you'll have a series of flight log files with a .TXT extension. You'll need to decode these with
|
||||
|
|
|
@ -23,8 +23,8 @@ The 8 pin RC_Input connector has the following pinouts when used in RX_PPM/RX_SE
|
|||
| 1 | Ground | |
|
||||
| 2 | +5V | |
|
||||
| 3 | PPM Input | Enable `feature RX_PPM` |
|
||||
| 4 | SoftSerial1 TX | Enable `feature SOFTSERIAL` |
|
||||
| 5 | SoftSerial1 RX | Enable `feature SOFTSERIAL` |
|
||||
| 4 | SoftSerial1 TX / Sonar trigger | |
|
||||
| 5 | SoftSerial1 RX / Sonar Echo | |
|
||||
| 6 | Current | Enable `feature CURRENT_METER`. Connect to the output of a current sensor, 0v-3.3v input |
|
||||
| 7 | Battery Voltage sensor | Enable `feature VBAT`. Connect to main battery using a voltage divider, 0v-3.3v input |
|
||||
| 8 | RSSI | Enable `feature RSSI_ADC`. Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input |
|
||||
|
|
|
@ -19,6 +19,8 @@ Currently the only supported sensor is the HCSR04 sensor.
|
|||
| Parallel PWM/ADC current sensor | PB8 / Motor 5 | PB9 / Motor 6 | NO (5v tolerant) |
|
||||
| PPM/Serial RX | PB0 / RC7 | PB1 / RC8 | YES (3.3v input) |
|
||||
|
||||
#### Constraints
|
||||
|
||||
Current meter cannot be used in conjunction with Parallel PWM and Sonar.
|
||||
|
||||
### Olimexino
|
||||
|
@ -27,4 +29,16 @@ Current meter cannot be used in conjunction with Parallel PWM and Sonar.
|
|||
| ------------- | ------------- | ------------------- |
|
||||
| PB0 / RC7 | PB1 / RC8 | YES (3.3v input) |
|
||||
|
||||
Current meter cannot be used in conjunction with sonar.
|
||||
#### Constraints
|
||||
|
||||
Current meter cannot be used in conjunction with Sonar.
|
||||
|
||||
### CC3D
|
||||
|
||||
| Trigger | Echo | Inline 1k resistors |
|
||||
| ------------- | ------------- | ------------------- |
|
||||
| PB5 | PB0 | YES (3.3v input) |
|
||||
|
||||
#### Constraints
|
||||
|
||||
Sonar cannot be used in conjuction with SoftSerial or Parallel PWM.
|
||||
|
|
|
@ -250,7 +250,6 @@ typedef enum BlackboxState {
|
|||
BLACKBOX_STATE_SEND_GPS_H_HEADERS,
|
||||
BLACKBOX_STATE_SEND_GPS_G_HEADERS,
|
||||
BLACKBOX_STATE_SEND_SYSINFO,
|
||||
BLACKBOX_STATE_PRERUN,
|
||||
BLACKBOX_STATE_RUNNING,
|
||||
BLACKBOX_STATE_SHUTTING_DOWN
|
||||
} BlackboxState;
|
||||
|
@ -268,6 +267,8 @@ extern uint32_t currentTime;
|
|||
|
||||
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
|
||||
|
||||
static uint32_t blackboxLastArmingBeep = 0;
|
||||
|
||||
static struct {
|
||||
uint32_t headerIndex;
|
||||
|
||||
|
@ -684,6 +685,12 @@ void startBlackbox(void)
|
|||
*/
|
||||
blackboxBuildConditionCache();
|
||||
|
||||
/*
|
||||
* Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
|
||||
* it finally plays the beep for this arming event.
|
||||
*/
|
||||
blackboxLastArmingBeep = getArmingBeepTimeMicros();
|
||||
|
||||
blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
|
||||
}
|
||||
}
|
||||
|
@ -1012,16 +1019,19 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
|
|||
}
|
||||
}
|
||||
|
||||
// Write the time of the last arming beep to the log as a synchronization point
|
||||
static void blackboxLogArmingBeep()
|
||||
/* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */
|
||||
static void blackboxCheckAndLogArmingBeep()
|
||||
{
|
||||
flightLogEvent_syncBeep_t eventData;
|
||||
|
||||
// Get time of last arming beep (in system-uptime microseconds)
|
||||
eventData.time = getArmingBeepTimeMicros();
|
||||
// Use != so that we can still detect a change if the counter wraps
|
||||
if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
|
||||
blackboxLastArmingBeep = getArmingBeepTimeMicros();
|
||||
|
||||
// Write the time to the log
|
||||
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
|
||||
eventData.time = blackboxLastArmingBeep;
|
||||
|
||||
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -1082,14 +1092,9 @@ void handleBlackbox(void)
|
|||
|
||||
//Keep writing chunks of the system info headers until it returns true to signal completion
|
||||
if (blackboxWriteSysinfo()) {
|
||||
blackboxSetState(BLACKBOX_STATE_PRERUN);
|
||||
blackboxSetState(BLACKBOX_STATE_RUNNING);
|
||||
}
|
||||
break;
|
||||
case BLACKBOX_STATE_PRERUN:
|
||||
blackboxSetState(BLACKBOX_STATE_RUNNING);
|
||||
|
||||
blackboxLogArmingBeep();
|
||||
break;
|
||||
case BLACKBOX_STATE_RUNNING:
|
||||
// On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
|
||||
|
||||
|
@ -1099,6 +1104,8 @@ void handleBlackbox(void)
|
|||
loadBlackboxState();
|
||||
writeIntraframe();
|
||||
} else {
|
||||
blackboxCheckAndLogArmingBeep();
|
||||
|
||||
/* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
|
||||
* recorded / skipped frames when the I frame's position is considered:
|
||||
*/
|
||||
|
|
|
@ -804,6 +804,12 @@ void validateAndFixConfig(void)
|
|||
masterConfig.mixerConfig.pid_at_min_throttle = 0;
|
||||
}
|
||||
|
||||
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
featureClear(FEATURE_SONAR);
|
||||
}
|
||||
#endif
|
||||
|
||||
useRxConfig(&masterConfig.rxConfig);
|
||||
|
||||
serialConfig_t *serialConfig = &masterConfig.serialConfig;
|
||||
|
|
|
@ -408,29 +408,6 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
|||
continue;
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
// skip Sonar pins
|
||||
// FIXME - Hack - See sonar.c sonarInit() and sonarHardware_t
|
||||
if (init->useSonar && timerHardwarePtr->gpio == GPIOB) {
|
||||
#if defined(SPRACINGF3) || defined(OLIMEXINO)
|
||||
if (timerHardwarePtr->pin == GPIO_Pin_0 || timerHardwarePtr->pin == GPIO_Pin_1) {
|
||||
continue;
|
||||
}
|
||||
#endif
|
||||
#if defined(NAZE)
|
||||
if (init->useParallelPWM) {
|
||||
if (timerHardwarePtr->pin == GPIO_Pin_8 || timerHardwarePtr->pin == GPIO_Pin_9) {
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
if (timerHardwarePtr->pin == GPIO_Pin_0 || timerHardwarePtr->pin == GPIO_Pin_1) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SOFTSERIAL_1_TIMER
|
||||
if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_1_TIMER)
|
||||
continue;
|
||||
|
@ -471,6 +448,17 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
if (init->sonarGPIOConfig && timerHardwarePtr->gpio == init->sonarGPIOConfig->gpio &&
|
||||
(
|
||||
timerHardwarePtr->pin == init->sonarGPIOConfig->triggerPin ||
|
||||
timerHardwarePtr->pin == init->sonarGPIOConfig->echoPin
|
||||
)
|
||||
) {
|
||||
continue;
|
||||
}
|
||||
#endif
|
||||
|
||||
// hacks to allow current functionality
|
||||
if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM)
|
||||
continue;
|
||||
|
|
|
@ -36,6 +36,12 @@
|
|||
#define PWM_TIMER_MHZ 1
|
||||
#define ONESHOT125_TIMER_MHZ 8
|
||||
|
||||
typedef struct sonarGPIOConfig_s {
|
||||
GPIO_TypeDef *gpio;
|
||||
uint16_t triggerPin;
|
||||
uint16_t echoPin;
|
||||
} sonarGPIOConfig_t;
|
||||
|
||||
typedef struct drv_pwm_config_t {
|
||||
bool useParallelPWM;
|
||||
bool usePPM;
|
||||
|
@ -65,6 +71,7 @@ typedef struct drv_pwm_config_t {
|
|||
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.
|
||||
sonarGPIOConfig_t *sonarGPIOConfig;
|
||||
} drv_pwm_config_t;
|
||||
|
||||
|
||||
|
|
|
@ -58,6 +58,11 @@ static void ECHO_EXTI_IRQHandler(void)
|
|||
EXTI_ClearITPendingBit(sonarHardware->exti_line);
|
||||
}
|
||||
|
||||
void EXTI0_IRQHandler(void)
|
||||
{
|
||||
ECHO_EXTI_IRQHandler();
|
||||
}
|
||||
|
||||
void EXTI1_IRQHandler(void)
|
||||
{
|
||||
ECHO_EXTI_IRQHandler();
|
||||
|
|
|
@ -25,6 +25,8 @@ typedef struct sonarHardware_s {
|
|||
IRQn_Type exti_irqn;
|
||||
} sonarHardware_t;
|
||||
|
||||
#define SONAR_GPIO GPIOB
|
||||
|
||||
void hcsr04_init(const sonarHardware_t *sonarHardware);
|
||||
|
||||
void hcsr04_start_reading(void);
|
||||
|
|
|
@ -60,7 +60,7 @@ typedef struct timerOvrHandlerRec_s {
|
|||
typedef struct {
|
||||
TIM_TypeDef *tim;
|
||||
GPIO_TypeDef *gpio;
|
||||
uint32_t pin;
|
||||
uint16_t pin;
|
||||
uint8_t channel;
|
||||
uint8_t irq;
|
||||
uint8_t outputEnable;
|
||||
|
|
|
@ -53,7 +53,7 @@
|
|||
static serialConfig_t *serialConfig;
|
||||
static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT];
|
||||
|
||||
serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
|
||||
const serialPortIdentifier_e const serialPortIdentifiers[SERIAL_PORT_COUNT] = {
|
||||
#ifdef USE_VCP
|
||||
SERIAL_PORT_USB_VCP,
|
||||
#endif
|
||||
|
@ -74,7 +74,9 @@ serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
|
|||
#endif
|
||||
};
|
||||
|
||||
uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e
|
||||
static uint8_t serialPortCount;
|
||||
|
||||
const uint32_t const baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e
|
||||
|
||||
#define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0]))
|
||||
|
||||
|
@ -226,16 +228,22 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool doesConfigurationUsePort(serialPortIdentifier_e identifier)
|
||||
serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier)
|
||||
{
|
||||
uint8_t index;
|
||||
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
|
||||
serialPortConfig_t *candidate = &serialConfig->portConfigs[index];
|
||||
if (candidate->identifier == identifier && candidate->functionMask) {
|
||||
return true;
|
||||
if (candidate->identifier == identifier) {
|
||||
return candidate;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
bool doesConfigurationUsePort(serialPortIdentifier_e identifier)
|
||||
{
|
||||
serialPortConfig_t *candidate = serialFindPortConfiguration(identifier);
|
||||
return candidate != NULL && candidate->functionMask;
|
||||
}
|
||||
|
||||
serialPort_t *openSerialPort(
|
||||
|
@ -324,6 +332,7 @@ void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled)
|
|||
|
||||
serialConfig = initialSerialConfig;
|
||||
|
||||
serialPortCount = SERIAL_PORT_COUNT;
|
||||
memset(&serialPortUsageList, 0, sizeof(serialPortUsageList));
|
||||
|
||||
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
|
||||
|
@ -339,11 +348,37 @@ void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled)
|
|||
#endif
|
||||
) {
|
||||
serialPortUsageList[index].identifier = SERIAL_PORT_NONE;
|
||||
serialPortCount--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void serialRemovePort(serialPortIdentifier_e identifier)
|
||||
{
|
||||
for (uint8_t index = 0; index < SERIAL_PORT_COUNT; index++) {
|
||||
if (serialPortUsageList[index].identifier == identifier) {
|
||||
serialPortUsageList[index].identifier = SERIAL_PORT_NONE;
|
||||
serialPortCount--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t serialGetAvailablePortCount(void)
|
||||
{
|
||||
return serialPortCount;
|
||||
}
|
||||
|
||||
bool serialIsPortAvailable(serialPortIdentifier_e identifier)
|
||||
{
|
||||
for (uint8_t index = 0; index < SERIAL_PORT_COUNT; index++) {
|
||||
if (serialPortUsageList[index].identifier == identifier) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void handleSerial(void)
|
||||
{
|
||||
#ifdef USE_CLI
|
||||
|
|
|
@ -46,7 +46,7 @@ typedef enum {
|
|||
BAUD_250000,
|
||||
} baudRate_e;
|
||||
|
||||
extern uint32_t baudRates[];
|
||||
extern const uint32_t const baudRates[];
|
||||
|
||||
// serial port identifiers are now fixed, these values are used by MSP commands.
|
||||
typedef enum {
|
||||
|
@ -61,7 +61,7 @@ typedef enum {
|
|||
SERIAL_PORT_IDENTIFIER_MAX = SERIAL_PORT_SOFTSERIAL2
|
||||
} serialPortIdentifier_e;
|
||||
|
||||
extern serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT];
|
||||
extern const serialPortIdentifier_e const serialPortIdentifiers[SERIAL_PORT_COUNT];
|
||||
|
||||
//
|
||||
// runtime
|
||||
|
@ -96,7 +96,11 @@ typedef struct serialConfig_s {
|
|||
//
|
||||
// configuration
|
||||
//
|
||||
void serialRemovePort(serialPortIdentifier_e identifier);
|
||||
uint8_t serialGetAvailablePortCount(void);
|
||||
bool serialIsPortAvailable(serialPortIdentifier_e identifier);
|
||||
bool isSerialConfigValid(serialConfig_t *serialConfig);
|
||||
serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier);
|
||||
bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier);
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function);
|
||||
serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function);
|
||||
|
@ -123,6 +127,7 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort);
|
|||
|
||||
baudRate_e lookupBaudRateIndex(uint32_t baudRate);
|
||||
|
||||
|
||||
//
|
||||
// msp/cli/bootloader
|
||||
//
|
||||
|
|
|
@ -1183,9 +1183,12 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
|
||||
case MSP_CF_SERIAL_CONFIG:
|
||||
headSerialReply(
|
||||
((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * SERIAL_PORT_COUNT)
|
||||
((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * serialGetAvailablePortCount())
|
||||
);
|
||||
for (i = 0; i < SERIAL_PORT_COUNT; i++) {
|
||||
if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
|
||||
continue;
|
||||
};
|
||||
serialize8(masterConfig.serialConfig.portConfigs[i].identifier);
|
||||
serialize16(masterConfig.serialConfig.portConfigs[i].functionMask);
|
||||
serialize8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
|
||||
|
@ -1595,17 +1598,28 @@ static bool processInCommand(void)
|
|||
{
|
||||
uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
|
||||
|
||||
if ((SERIAL_PORT_COUNT * portConfigSize) != currentPort->dataSize) {
|
||||
if (currentPort->dataSize % portConfigSize != 0) {
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
for (i = 0; i < SERIAL_PORT_COUNT; i++) {
|
||||
masterConfig.serialConfig.portConfigs[i].identifier = read8();
|
||||
masterConfig.serialConfig.portConfigs[i].functionMask = read16();
|
||||
masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex = read8();
|
||||
masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex = read8();
|
||||
masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex = read8();
|
||||
masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex = read8();
|
||||
|
||||
uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize;
|
||||
|
||||
while (remainingPortsInPacket--) {
|
||||
uint8_t identifier = read8();
|
||||
|
||||
serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
|
||||
if (!portConfig) {
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
|
||||
portConfig->identifier = identifier;
|
||||
portConfig->functionMask = read16();
|
||||
portConfig->msp_baudrateIndex = read8();
|
||||
portConfig->gps_baudrateIndex = read8();
|
||||
portConfig->telemetry_baudrateIndex = read8();
|
||||
portConfig->blackbox_baudrateIndex = read8();
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
|
|
@ -47,6 +47,7 @@
|
|||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
@ -114,6 +115,8 @@ void displayInit(rxConfig_t *intialRxConfig);
|
|||
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
|
||||
void loop(void);
|
||||
void spektrumBind(rxConfig_t *rxConfig);
|
||||
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
|
||||
void sonarInit(const sonarHardware_t *sonarHardware);
|
||||
|
||||
#ifdef STM32F303xC
|
||||
// from system_stm32f30x.c
|
||||
|
@ -193,6 +196,21 @@ void init(void)
|
|||
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
|
||||
|
||||
memset(&pwm_params, 0, sizeof(pwm_params));
|
||||
|
||||
#ifdef SONAR
|
||||
const sonarHardware_t *sonarHardware = NULL;
|
||||
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
|
||||
sonarGPIOConfig_t sonarGPIOConfig = {
|
||||
.echoPin = sonarHardware->trigger_pin,
|
||||
.triggerPin = sonarHardware->echo_pin,
|
||||
.gpio = SONAR_GPIO
|
||||
};
|
||||
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
|
||||
}
|
||||
#endif
|
||||
|
||||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
||||
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
|
||||
pwm_params.airplane = true;
|
||||
|
@ -278,10 +296,22 @@ void init(void)
|
|||
updateHardwareRevision();
|
||||
#endif
|
||||
|
||||
#if defined(NAZE)
|
||||
if (hardwareRevision == NAZE32_SP) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
|
||||
} else {
|
||||
serialRemovePort(SERIAL_PORT_USART3);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_I2C
|
||||
#if defined(NAZE)
|
||||
if (hardwareRevision != NAZE32_SP) {
|
||||
i2cInit(I2C_DEVICE);
|
||||
} else {
|
||||
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
|
||||
i2cInit(I2C_DEVICE);
|
||||
}
|
||||
}
|
||||
#elif defined(CC3D)
|
||||
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
|
||||
|
@ -371,7 +401,7 @@ void init(void)
|
|||
|
||||
#ifdef SONAR
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
sonarInit(&masterConfig.batteryConfig);
|
||||
sonarInit(sonarHardware);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -38,7 +38,7 @@
|
|||
|
||||
static int32_t calculatedAltitude;
|
||||
|
||||
void sonarInit(batteryConfig_t *batteryConfig)
|
||||
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
|
||||
{
|
||||
#if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
|
||||
static const sonarHardware_t const sonarPWM56 = {
|
||||
|
@ -57,9 +57,9 @@ void sonarInit(batteryConfig_t *batteryConfig)
|
|||
};
|
||||
// If we are using parallel PWM for our receiver or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
|
||||
if (feature(FEATURE_RX_PARALLEL_PWM ) || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC) ) {
|
||||
hcsr04_init(&sonarPWM56);
|
||||
return &sonarPWM56;
|
||||
} else {
|
||||
hcsr04_init(&sonarRC78);
|
||||
return &sonarRC78;
|
||||
}
|
||||
#elif defined(OLIMEXINO)
|
||||
UNUSED(batteryConfig);
|
||||
|
@ -70,7 +70,17 @@ void sonarInit(batteryConfig_t *batteryConfig)
|
|||
.exti_pin_source = GPIO_PinSource1,
|
||||
.exti_irqn = EXTI1_IRQn
|
||||
};
|
||||
hcsr04_init(&sonarHardware);
|
||||
return &sonarHardware;
|
||||
#elif defined(CC3D)
|
||||
UNUSED(batteryConfig);
|
||||
static const sonarHardware_t const sonarHardware = {
|
||||
.trigger_pin = Pin_5, // (PB5)
|
||||
.echo_pin = Pin_0, // (PB0) - only 3.3v ( add a 1K Ohms resistor )
|
||||
.exti_line = EXTI_Line0,
|
||||
.exti_pin_source = GPIO_PinSource0,
|
||||
.exti_irqn = EXTI0_IRQn
|
||||
};
|
||||
return &sonarHardware;
|
||||
#elif defined(SPRACINGF3)
|
||||
UNUSED(batteryConfig);
|
||||
static const sonarHardware_t const sonarHardware = {
|
||||
|
@ -80,11 +90,15 @@ void sonarInit(batteryConfig_t *batteryConfig)
|
|||
.exti_pin_source = EXTI_PinSource1,
|
||||
.exti_irqn = EXTI1_IRQn
|
||||
};
|
||||
hcsr04_init(&sonarHardware);
|
||||
return &sonarHardware;
|
||||
#else
|
||||
#error Sonar not defined for target
|
||||
#endif
|
||||
}
|
||||
|
||||
void sonarInit(const sonarHardware_t *sonarHardware)
|
||||
{
|
||||
hcsr04_init(sonarHardware);
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
calculatedAltitude = -1;
|
||||
}
|
||||
|
|
|
@ -18,9 +18,9 @@
|
|||
#pragma once
|
||||
#include "sensors/battery.h"
|
||||
|
||||
void sonarInit(batteryConfig_t *batteryConfig);
|
||||
void sonarUpdate(void);
|
||||
|
||||
int32_t sonarRead(void);
|
||||
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
|
||||
int32_t sonarGetLatestAltitude(void);
|
||||
|
||||
|
|
|
@ -111,6 +111,7 @@
|
|||
#define BLACKBOX
|
||||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define SONAR
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
#define USE_CLI
|
||||
|
@ -119,6 +120,7 @@
|
|||
#undef AUTOTUNE // disabled for OPBL build due to code size.
|
||||
#endif
|
||||
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART3, PB11 (Flexport)
|
||||
#define BIND_PORT GPIOB
|
||||
|
|
|
@ -119,9 +119,10 @@
|
|||
|
||||
#define USE_USART1
|
||||
#define USE_USART2
|
||||
#define USE_USART3
|
||||
#define USE_SOFTSERIAL1
|
||||
#define USE_SOFTSERIAL2
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
#define SERIAL_PORT_COUNT 5
|
||||
|
||||
#define SOFTSERIAL_1_TIMER TIM3
|
||||
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
|
||||
|
@ -130,6 +131,13 @@
|
|||
#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
|
||||
#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
|
||||
|
||||
// USART3 only on NAZE32_SP - Flex Port
|
||||
#define USART3_RX_PIN Pin_11
|
||||
#define USART3_TX_PIN Pin_10
|
||||
#define USART3_GPIO GPIOB
|
||||
#define USART3_APB1_PERIPHERALS RCC_APB1Periph_USART3
|
||||
#define USART3_APB2_PERIPHERALS RCC_APB2Periph_GPIOB
|
||||
|
||||
#define USE_I2C
|
||||
#define I2C_DEVICE (I2CDEV_2)
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue