Merge branch 'master' into dev

This commit is contained in:
Michael Hope 2015-05-23 05:25:41 +02:00
commit 4658ff5b3c
19 changed files with 209 additions and 70 deletions

View file

@ -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 \

View file

@ -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.
![Dataflash tab in Configurator](Screenshots/blackbox-dataflash.png)
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

View file

@ -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 |

View file

@ -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.

View file

@ -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:
*/

View file

@ -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;

View file

@ -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;

View file

@ -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;

View file

@ -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();

View file

@ -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);

View file

@ -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;

View file

@ -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

View file

@ -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
//

View file

@ -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;

View file

@ -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

View file

@ -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;
}

View file

@ -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);

View file

@ -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

View file

@ -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)