This commit is contained in:
KiteAnton 2016-08-01 17:09:06 +02:00
commit c01b45a002
16 changed files with 294 additions and 62 deletions

View File

@ -530,11 +530,7 @@ static void resetConf(void)
masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF;
masterConfig.rxConfig.rcSmoothInterval = 9;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
#ifdef STM32F4
masterConfig.rxConfig.max_aux_channel = 99;
#else
masterConfig.rxConfig.max_aux_channel = 6;
#endif
masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS;
masterConfig.rxConfig.airModeActivateThreshold = 1350;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

View File

@ -196,12 +196,27 @@ static bool m25p16_readIdentification()
* Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
* m25p16_getGeometry().
*/
bool m25p16_init()
bool m25p16_init(ioTag_t csTag)
{
/*
if we have already detected a flash device we can simply exit
TODO: change the init param in favour of flash CFG when ParamGroups work is done
then cs pin can be specified in hardware_revision.c or config.c (dependent on revision).
*/
if (geometry.sectors) {
return true;
}
if (csTag) {
m25p16CsPin = IOGetByTag(csTag);
} else {
#ifdef M25P16_CS_PIN
m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN));
m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN));
#else
return false;
#endif
}
IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0);
IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG);

View File

@ -19,10 +19,11 @@
#include <stdint.h>
#include "flash.h"
#include "io.h"
#define M25P16_PAGESIZE 256
bool m25p16_init();
bool m25p16_init(ioTag_t csTag);
void m25p16_eraseSector(uint32_t address);
void m25p16_eraseCompletely();

View File

@ -27,11 +27,16 @@
#include "inverter.h"
static const IO_t pin = DEFIO_IO(INVERTER);
/*
TODO: move this to support multiple inverters on different UARTs etc
possibly move to put it in the UART driver itself.
*/
static IO_t pin = IO_NONE;
void initInverter(void)
{
IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 0);
pin = IOGetByTag(IO_TAG(INVERTER));
IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 1);
IOConfigGPIO(pin, IOCFG_OUT_PP);
inverterSet(false);

View File

@ -11,6 +11,9 @@
typedef uint8_t ioTag_t; // packet tag to specify IO pin
typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change
// NONE initializer for ioTag_t variables
#define IOTAG_NONE ((ioTag_t)0)
// NONE initializer for IO_t variable
#define IO_NONE ((IO_t)0)

View File

@ -66,9 +66,10 @@ uint8_t PIDweight[3];
static int32_t errorGyroI[3];
static float errorGyroIf[3];
#ifdef SKIP_PID_FLOAT
static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
#ifdef SKIP_PID_FLOAT
pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using
#else
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
@ -107,6 +108,7 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
#ifndef SKIP_PID_FLOAT
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab)
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
@ -302,6 +304,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
#endif
}
}
#endif
// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future
static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,

View File

@ -826,7 +826,7 @@ const clivalue_t valueTable[] = {
{ "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } },
{ "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
@ -2009,11 +2009,6 @@ static void dumpValues(uint16_t valueSection)
cliPrintf("set %s = ", valueTable[i].name);
cliPrintVar(value, 0);
cliPrint("\r\n");
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
}
@ -2058,7 +2053,7 @@ static void cliDump(char *cmdline)
cliPrint("\r\n# version\r\n");
cliVersion(NULL);
printSectionBreak();
printSectionBreak();
cliPrint("\r\n# name\r\n");
cliName(NULL);
@ -2089,9 +2084,6 @@ static void cliDump(char *cmdline)
if (yaw < 0)
cliWrite(' ');
cliPrintf("%s\r\n", ftoa(yaw, buf));
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
#ifdef USE_SERVOS
@ -2113,10 +2105,6 @@ static void cliDump(char *cmdline)
masterConfig.customServoMixer[i].max,
masterConfig.customServoMixer[i].box
);
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
#endif
@ -2128,18 +2116,12 @@ static void cliDump(char *cmdline)
if (featureNames[i] == NULL)
break;
cliPrintf("feature -%s\r\n", featureNames[i]);
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
for (i = 0; ; i++) { // reenable what we want.
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
cliPrintf("feature %s\r\n", featureNames[i]);
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
#ifdef BEEPER
@ -2194,9 +2176,6 @@ static void cliDump(char *cmdline)
for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) {
if (servoDirection(i, channel) < 0) {
cliPrintf("smix reverse %d %d r\r\n", i , channel);
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
}
}
@ -2227,9 +2206,6 @@ static void cliDump(char *cmdline)
cliPrint("\r\n# restore original rateprofile selection\r\n");
changeControlRateProfile(currentRateIndex);
cliRateProfile("");
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
cliPrint("\r\n# restore original profile selection\r\n");
@ -2710,6 +2686,10 @@ static void cliPrint(const char *str)
{
while (*str)
bufWriterAppend(cliWriter, *str++);
#ifdef USE_SLOW_SERIAL_CLI
delay(1);
#endif
}
static void cliPutp(void *p, char ch)
@ -2723,6 +2703,10 @@ static void cliPrintf(const char *fmt, ...)
va_start(va, fmt);
tfp_format(cliWriter, cliPutp, fmt, va);
va_end(va);
#ifdef USE_SLOW_SERIAL_CLI
delay(1);
#endif
}
static void cliWrite(uint8_t ch)
@ -2856,10 +2840,6 @@ static void cliSet(char *cmdline)
cliPrintf("%s = ", valueTable[i].name);
cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
cliPrint("\r\n");
#ifdef USE_SLOW_SERIAL_CLI
delay(2);
#endif
}
} else if ((eqptr = strstr(cmdline, "=")) != NULL) {
// has equals

View File

@ -179,8 +179,7 @@ void init(void)
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_1) {
ledInit(false);
}
else {
} else {
ledInit(true);
}
#else
@ -363,6 +362,12 @@ void init(void)
beeperConfig.isInverted = true;
}
#endif
/* temp until PGs are implemented. */
#ifdef BLUEJAYF4
if (hardwareRevision <= BJF4_REV2) {
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
}
#endif
#ifdef CC3D
if (masterConfig.use_buzzer_p6 == 1)
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
@ -581,10 +586,10 @@ void init(void)
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
m25p16_init(IOTAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
m25p16_init(IOTAG_NONE);
#endif
flashfsInit();
@ -599,7 +604,11 @@ void init(void)
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
#ifdef STM32F4
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif

View File

@ -76,7 +76,8 @@
#include "config/config_master.h"
// alternative defaults settings for AlienFlight targets
void targetConfiguration(void) {
void targetConfiguration(void)
{
featureClear(FEATURE_ONESHOT125);
masterConfig.rxConfig.spektrum_sat_bind = 5;
masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1;

View File

@ -0,0 +1,87 @@
/*
* 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/>.
*/
#include <stdint.h>
#include <platform.h>
#include "build_config.h"
#include "blackbox/blackbox_io.h"
#include "common/color.h"
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/system.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/serial.h"
#include "drivers/pwm_output.h"
#include "drivers/max7456.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "io/ledstrip.h"
#include "io/gps.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "hardware_revision.h"
// alternative defaults settings for BlueJayF4 targets
void targetConfiguration(void)
{
if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) {
masterConfig.sensorAlignmentConfig.gyro_align = CW180_DEG;
masterConfig.sensorAlignmentConfig.acc_align = CW180_DEG;
}
}

View File

@ -0,0 +1,99 @@
/*
* 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/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "build_config.h"
#include "drivers/system.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/flash_m25p16.h"
#include "hardware_revision.h"
static const char * const hardwareRevisionNames[] = {
"Unknown",
"BlueJay rev1",
"BlueJay rev2",
"BlueJay rev3",
"BlueJay rev3a"
};
uint8_t hardwareRevision = UNKNOWN;
void detectHardwareRevision(void)
{
IO_t pin1 = IOGetByTag(IO_TAG(PB12));
IOInit(pin1, OWNER_SYSTEM, RESOURCE_INPUT, 1);
IOConfigGPIO(pin1, IOCFG_IPU);
IO_t pin2 = IOGetByTag(IO_TAG(PB13));
IOInit(pin2, OWNER_SYSTEM, RESOURCE_INPUT, 2);
IOConfigGPIO(pin2, IOCFG_IPU);
// Check hardware revision
delayMicroseconds(10); // allow configuration to settle
/*
if both PB12 and 13 are tied to GND then it is Rev3A (mini)
if only PB12 is tied to GND then it is a Rev3 (full size)
*/
if (!IORead(pin1)) {
if (!IORead(pin2)) {
hardwareRevision = BJF4_REV3A;
}
hardwareRevision = BJF4_REV3;
}
if (hardwareRevision == UNKNOWN) {
hardwareRevision = BJF4_REV2;
return;
}
/*
enable the UART1 inversion PC9
TODO: once param groups are in place, inverter outputs
can be moved to be simple IO outputs, and merely set them
HI or LO in configuration.
*/
IO_t uart1invert = IOGetByTag(IO_TAG(PC9));
IOInit(uart1invert, OWNER_INVERTER, RESOURCE_OUTPUT, 2);
IOConfigGPIO(uart1invert, IOCFG_AF_PP);
IOLo(uart1invert);
}
void updateHardwareRevision(void)
{
if (hardwareRevision != BJF4_REV2) {
return;
}
/*
if flash exists on PB3 then Rev1
*/
if (m25p16_init(IO_TAG(PB3))) {
hardwareRevision = BJF4_REV1;
} else {
IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, RESOURCE_NONE, 0);
}
}

View File

@ -0,0 +1,30 @@
/*
* 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
typedef enum bjf4HardwareRevision_t {
UNKNOWN = 0,
BJF4_REV1, // Flash
BJF4_REV2, // SDCard
BJF4_REV3, // SDCard + Flash
BJF4_REV3A, // Flash (20x20 mini format)
} bjf4HardwareRevision_e;
extern uint8_t hardwareRevision;
void updateHardwareRevision(void);
void detectHardwareRevision(void);

View File

@ -17,11 +17,15 @@
#pragma once
#define TARGET_BOARD_IDENTIFIER "BJF4"
#define TARGET_CONFIG
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
#define USBD_PRODUCT_STRING "BlueJayF4"
#define USE_HARDWARE_REVISION_DETECTION
#define HW_PIN PB2
#define BOARD_HAS_VOLTAGE_DIVIDER
#define USE_EXTI
@ -29,7 +33,8 @@
#define LED1 PB5
#define LED2 PB4
#define BEEPER PB7
#define BEEPER PC1
#define BEEPER_OPT PB7
#define BEEPER_INVERTED
#define INVERTER PB15
@ -41,12 +46,12 @@
#define ACC
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW180_DEG
#define ACC_MPU6500_ALIGN CW0_DEG
#define GYRO
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6500_ALIGN CW180_DEG
#define GYRO_MPU6500_ALIGN CW0_DEG
//#define MAG
//#define USE_MAG_AK8963
@ -76,11 +81,11 @@
// Performance logging for SD card operations:
// #define AFATFS_USE_INTROSPECTIVE_LOGGING
//#define M25P16_CS_PIN PB3
//#define M25P16_SPI_INSTANCE SPI3
#define M25P16_CS_PIN PB7
#define M25P16_SPI_INSTANCE SPI3
//#define USE_FLASHFS
//#define USE_FLASH_M25P16
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define USABLE_TIMER_CHANNEL_COUNT 7

View File

@ -1,5 +1,5 @@
F405_TARGETS += $(TARGET)
FEATURES += SDCARD VCP
FEATURES += SDCARD VCP ONBOARDFLASH
TARGET_SRC = \
drivers/accgyro_spi_mpu6500.c \

View File

@ -57,8 +57,7 @@
#define USE_UART2
#define USE_UART3
#define USE_SOFTSERIAL1
#define USE_SOFTSERIAL2
#define SERIAL_PORT_COUNT 5
#define SERIAL_PORT_COUNT 4
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
@ -70,11 +69,8 @@
#define UART3_RX_PIN PB11 // PB11 (AF7)
#define SOFTSERIAL_1_TIMER TIM3
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
#define SOFTSERIAL_2_TIMER TIM3
#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 6 // PWM 5
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 7 // PWM 6
#define USE_I2C

View File

@ -24,6 +24,7 @@
/* STM32F4 specific settings that apply to all F4 targets */
#ifdef STM32F4
#define MAX_AUX_CHANNELS 99
#define TASK_GYROPID_DESIRED_PERIOD 125
#define SCHEDULER_DELAY_LIMIT 10
#define USE_SLOW_SERIAL_CLI
@ -31,6 +32,7 @@
#else /* when not an F4 */
#define MAX_AUX_CHANNELS 6
#define TASK_GYROPID_DESIRED_PERIOD 1000
#define SCHEDULER_DELAY_LIMIT 100