From 2df7e3cefae52ef7018bb93f2a824247bd574a38 Mon Sep 17 00:00:00 2001 From: sblakemore Date: Wed, 2 Dec 2015 08:56:28 +1000 Subject: [PATCH 1/3] Add RTC6705 SPI VTX support --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/config/config.c | 8 ++ src/main/config/config_master.h | 9 ++ src/main/drivers/vtx_rtc6705.c | 191 ++++++++++++++++++++++++++++++++ src/main/drivers/vtx_rtc6705.h | 38 +++++++ src/main/io/beeper.c | 1 + src/main/io/ledstrip.c | 1 + src/main/io/rc_controls.c | 16 +++ src/main/io/rc_controls.h | 1 + src/main/io/serial_cli.c | 82 ++++++++++++++ src/main/io/serial_msp.c | 1 + src/main/io/vtx.c | 146 ++++++++++++++++++++++++ src/main/io/vtx.h | 40 +++++++ src/main/main.c | 5 + src/main/mw.c | 5 +- src/main/telemetry/ltm.c | 1 + src/main/telemetry/smartport.c | 1 + 18 files changed, 547 insertions(+), 1 deletion(-) create mode 100644 src/main/drivers/vtx_rtc6705.c create mode 100644 src/main/drivers/vtx_rtc6705.h create mode 100644 src/main/io/vtx.c create mode 100644 src/main/io/vtx.h diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 97ca2b619..9e8fdc1fd 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -60,6 +60,7 @@ #include "io/serial_cli.h" #include "io/serial_msp.h" #include "io/statusindicator.h" +#include "io/vtx.h" #include "rx/rx.h" #include "rx/msp.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 916b22ba2..0eeead315 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -36,6 +36,7 @@ #include "io/escservo.h" #include "rx/rx.h" #include "io/rc_controls.h" +#include "io/vtx.h" #include "io/gimbal.h" #include "io/gps.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index f7d1cb57a..7a5d3d153 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -54,6 +54,7 @@ #include "io/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" +#include "io/vtx.h" #include "rx/rx.h" @@ -567,6 +568,13 @@ static void resetConf(void) masterConfig.ledstrip_visual_beeper = 0; #endif +#ifdef VTX + masterConfig.vtx_band = 4; //Fatshark/Airwaves + masterConfig.vtx_channel = 1; //CH1 + masterConfig.vtx_mode = 0; //CH+BAND mode + masterConfig.vtx_mhz = 5740; //F0 +#endif + #ifdef SPRACINGF3 featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = 1; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index ecc4af7ec..ee3412202 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -132,6 +132,15 @@ typedef struct master_t { modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; +#ifdef VTX + uint8_t vtx_band; //1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband + uint8_t vtx_channel; //1-8 + uint8_t vtx_mode; //0=ch+band 1=mhz + uint16_t vtx_mhz; //5740 + + vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; +#endif + #ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c new file mode 100644 index 000000000..4bdaf9931 --- /dev/null +++ b/src/main/drivers/vtx_rtc6705.c @@ -0,0 +1,191 @@ +/* + * 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 . + */ + +/* + * Author: Giles Burgess (giles@multiflite.co.uk) + * + * This source code is provided as is and can be used/modified so long + * as this header is maintained with the file at all times. + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef VTX + +#include "common/maths.h" + +#include "drivers/vtx_rtc6705.h" +#include "drivers/bus_spi.h" +#include "drivers/system.h" + +#define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 +#define RTC6705_SET_A1 0x8F3031 //5865 +#define RTC6705_SET_A2 0x8EB1B1 //5845 +#define RTC6705_SET_A3 0x8E3331 //5825 +#define RTC6705_SET_A4 0x8DB4B1 //5805 +#define RTC6705_SET_A5 0x8D3631 //5785 +#define RTC6705_SET_A6 0x8CB7B1 //5765 +#define RTC6705_SET_A7 0x8C4131 //5745 +#define RTC6705_SET_A8 0x8BC2B1 //5725 +#define RTC6705_SET_B1 0x8BF3B1 //5733 +#define RTC6705_SET_B2 0x8C6711 //5752 +#define RTC6705_SET_B3 0x8CE271 //5771 +#define RTC6705_SET_B4 0x8D55D1 //5790 +#define RTC6705_SET_B5 0x8DD131 //5809 +#define RTC6705_SET_B6 0x8E4491 //5828 +#define RTC6705_SET_B7 0x8EB7F1 //5847 +#define RTC6705_SET_B8 0x8F3351 //5866 +#define RTC6705_SET_E1 0x8B4431 //5705 +#define RTC6705_SET_E2 0x8AC5B1 //5685 +#define RTC6705_SET_E3 0x8A4731 //5665 +#define RTC6705_SET_E4 0x89D0B1 //5645 +#define RTC6705_SET_E5 0x8FA6B1 //5885 +#define RTC6705_SET_E6 0x902531 //5905 +#define RTC6705_SET_E7 0x90A3B1 //5925 +#define RTC6705_SET_E8 0x912231 //5945 +#define RTC6705_SET_F1 0x8C2191 //5740 +#define RTC6705_SET_F2 0x8CA011 //5760 +#define RTC6705_SET_F3 0x8D1691 //5780 +#define RTC6705_SET_F4 0x8D9511 //5800 +#define RTC6705_SET_F5 0x8E1391 //5820 +#define RTC6705_SET_F6 0x8E9211 //5840 +#define RTC6705_SET_F7 0x8F1091 //5860 +#define RTC6705_SET_F8 0x8F8711 //5880 +#define RTC6705_SET_R1 0x8A2151 //5658 +#define RTC6705_SET_R2 0x8B04F1 //5695 +#define RTC6705_SET_R3 0x8BF091 //5732 +#define RTC6705_SET_R4 0x8CD431 //5769 +#define RTC6705_SET_R5 0x8DB7D1 //5806 +#define RTC6705_SET_R6 0x8EA371 //5843 +#define RTC6705_SET_R7 0x8F8711 //5880 +#define RTC6705_SET_R8 0x9072B1 //5917 + +#define RTC6705_SET_R 400 //Reference clock +#define RTC6705_SET_FDIV 1024 //128*(fosc/1000000) +#define RTC6705_SET_NDIV 16 //Remainder divider to get 'A' part of equation +#define RTC6705_SET_WRITE 0x11 //10001b to write to register +#define RTC6705_SET_DIVMULT 1000000 //Division value (to fit into a uint32_t) (Hz to MHz) + +#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN) +#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN) + +// Define variables +static const uint32_t channelArray[RTC6705_BAND_MAX][RTC6705_CHANNEL_MAX] = { + { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 }, + { RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 }, + { RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 }, + { RTC6705_SET_F1, RTC6705_SET_F2, RTC6705_SET_F3, RTC6705_SET_F4, RTC6705_SET_F5, RTC6705_SET_F6, RTC6705_SET_F7, RTC6705_SET_F8 }, + { RTC6705_SET_R1, RTC6705_SET_R2, RTC6705_SET_R3, RTC6705_SET_R4, RTC6705_SET_R5, RTC6705_SET_R6, RTC6705_SET_R7, RTC6705_SET_R8 }, +}; + +/** + * Send a command and return if good + * TODO chip detect + */ +static bool rtc6705IsReady() +{ + // Sleep a little bit to make sure it has booted + delay(50); + + // TODO Do a read and get current config (note this would be reading on MOSI (data) line) + + return true; +} + +/** + * Reverse a uint32_t (LSB to MSB) + * This is easier for when generating the frequency to then + * reverse the bits afterwards + */ +static uint32_t reverse32(uint32_t in) { + uint32_t out = 0; + + for (uint8_t i = 0 ; i < 32 ; i++) + { + out |= ((in>>i) & 1)<<(31-i); + } + + return out; +} + +/** + * Start chip if available + */ +bool rtc6705Init() +{ + DISABLE_RTC6705; + spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + return rtc6705IsReady(); +} + +/** + * Transfer a 25bit packet to RTC6705 + * This will just send it as a 32bit packet LSB meaning + * extra 0's get truncated on RTC6705 end + */ +static void rtc6705Transfer(uint32_t command) +{ + command = reverse32(command); + + ENABLE_RTC6705; + + spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 24) & 0xFF); + spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 16) & 0xFF); + spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 8) & 0xFF); + spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 0) & 0xFF); + + DISABLE_RTC6705; +} + +/** + * Set a band and channel + */ +void rtc6705SetChannel(uint8_t band, uint8_t channel) +{ + band = constrain(band, RTC6705_BAND_MIN, RTC6705_BAND_MAX); + channel = constrain(channel, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); + + rtc6705Transfer(RTC6705_SET_HEAD); + rtc6705Transfer(channelArray[band-1][channel-1]); +} + + /** + * Set a freq in mhz + * Formula derived from datasheet + */ +void rtc6705SetFreq(uint16_t freq) +{ + freq = constrain(freq, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX); + + uint32_t val_hex = 0; + + uint32_t val_a = ((((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers) + uint32_t val_n = (((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers) + + val_hex |= RTC6705_SET_WRITE; + val_hex |= (val_a << 5); + val_hex |= (val_n << 12); + + rtc6705Transfer(RTC6705_SET_HEAD); + rtc6705Transfer(val_hex); +} + +#endif diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h new file mode 100644 index 000000000..d72116659 --- /dev/null +++ b/src/main/drivers/vtx_rtc6705.h @@ -0,0 +1,38 @@ +/* + * 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 . + */ + +/* + * Author: Giles Burgess (giles@multiflite.co.uk) + * + * This source code is provided as is and can be used/modified so long + * as this header is maintained with the file at all times. + */ + +#pragma once + +#include + +#define RTC6705_BAND_MIN 1 +#define RTC6705_BAND_MAX 5 +#define RTC6705_CHANNEL_MIN 1 +#define RTC6705_CHANNEL_MAX 8 +#define RTC6705_FREQ_MIN 5600 +#define RTC6705_FREQ_MAX 5950 + +bool rtc6705Init(); +void rtc6705SetChannel(uint8_t band, uint8_t channel); +void rtc6705SetFreq(uint16_t freq); \ No newline at end of file diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 1448a6b5e..f1ac9e352 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -31,6 +31,7 @@ #include "sensors/sensors.h" #include "io/statusindicator.h" +#include "io/vtx.h" #ifdef GPS #include "io/gps.h" diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index cf85b7db3..afb78f352 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -58,6 +58,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" +#include "io/vtx.h" #include "flight/failsafe.h" #include "flight/mixer.h" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index a72efd990..8f25d6f6c 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -49,6 +49,7 @@ #include "io/escservo.h" #include "io/rc_controls.h" #include "io/rc_curves.h" +#include "io/vtx.h" #include "io/display.h" @@ -304,6 +305,21 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat } #endif +#ifdef VTX + if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) { + vtxIncrementBand(); + } + if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) { + vtxDecrementBand(); + } + if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) { + vtxIncrementChannel(); + } + if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) { + vtxDecrementChannel(); + } +#endif + } bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId) diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 8cbb078a4..0f107e403 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -161,6 +161,7 @@ bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ea6801e07..7cc6ae32f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -60,6 +60,7 @@ #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/vtx.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -164,6 +165,10 @@ static void cliFlashRead(char *cmdline); #endif #endif +#ifdef VTX +static void cliVtx(char *cmdline); +#endif + #ifdef USE_SDCARD static void cliSdInfo(char *cmdline); #endif @@ -326,6 +331,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif +#ifdef VTX + CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), +#endif }; #define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) @@ -768,6 +776,13 @@ const clivalue_t valueTable[] = { { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, #endif +#ifdef VTX + { "vtx_band", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_band, .config.minmax = { 1, 5 } }, + { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 1, 8 } }, + { "vtx_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_mode, .config.minmax = { 0, 2 } }, + { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, &masterConfig.vtx_mhz, .config.minmax = { 5600, 5950 } }, +#endif + { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } }, { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, @@ -1773,6 +1788,67 @@ static void cliFlashRead(char *cmdline) #endif #endif +#ifdef VTX +static void cliVtx(char *cmdline) +{ + int i, val = 0; + char *ptr; + + if (isEmpty(cmdline)) { + // print out vtx channel settings + for (i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { + vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; + printf("vtx %u %u %u %u %u %u\r\n", + i, + cac->auxChannelIndex, + cac->band, + cac->channel, + MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) + ); + } + } else { + ptr = cmdline; + i = atoi(ptr++); + if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { + vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; + uint8_t validArgumentCount = 0; + ptr = strchr(ptr, ' '); + if (ptr) { + val = atoi(++ptr); + if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { + cac->auxChannelIndex = val; + validArgumentCount++; + } + } + ptr = strchr(ptr, ' '); + if (ptr) { + val = atoi(++ptr); + if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) { + cac->band = val; + validArgumentCount++; + } + } + ptr = strchr(ptr, ' '); + if (ptr) { + val = atoi(++ptr); + if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) { + cac->channel = val; + validArgumentCount++; + } + } + ptr = processChannelRangeArgs(ptr, &cac->range, &validArgumentCount); + + if (validArgumentCount != 5) { + memset(cac, 0, sizeof(vtxChannelActivationCondition_t)); + } + } else { + cliShowArgumentRangeError("index", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1); + } + } +} +#endif + static void dumpValues(uint16_t valueSection) { uint32_t i; @@ -1963,6 +2039,12 @@ static void cliDump(char *cmdline) } #endif +#ifdef VTX + cliPrint("\r\n# vtx\r\n"); + + cliVtx(""); +#endif + printSectionBreak(); dumpValues(MASTER_VALUE); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a65b224b8..f932fdb80 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -57,6 +57,7 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/vtx.h" #include "telemetry/telemetry.h" diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c new file mode 100644 index 000000000..3a48bc1a6 --- /dev/null +++ b/src/main/io/vtx.c @@ -0,0 +1,146 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef VTX + +#include "common/color.h" +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.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/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/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/runtime_config.h" + +static uint8_t locked = 0; + +void vtxInit() +{ + rtc6705Init(); + if (masterConfig.vtx_mode == 0) { + rtc6705SetChannel(masterConfig.vtx_band, masterConfig.vtx_channel); + } else if (masterConfig.vtx_mode == 1) { + rtc6705SetFreq(masterConfig.vtx_mhz); + } +} + +static void setChannelSaveAndNotify(uint8_t *bandOrChannel, uint8_t step, int32_t min, int32_t max) +{ + if (ARMING_FLAG(ARMED)) { + locked = 1; + } + + if (masterConfig.vtx_mode == 0 && !locked) { + uint8_t temp = (*bandOrChannel) + step; + temp = constrain(temp, min, max); + *bandOrChannel = temp; + + rtc6705SetChannel(masterConfig.vtx_band, masterConfig.vtx_channel); + writeEEPROM(); + readEEPROM(); + beeperConfirmationBeeps(temp); + } +} + +void vtxIncrementBand() +{ + setChannelSaveAndNotify(&(masterConfig.vtx_band), 1, RTC6705_BAND_MIN, RTC6705_BAND_MAX); +} + +void vtxDecrementBand() +{ + setChannelSaveAndNotify(&(masterConfig.vtx_band), -1, RTC6705_BAND_MIN, RTC6705_BAND_MAX); +} + +void vtxIncrementChannel() +{ + setChannelSaveAndNotify(&(masterConfig.vtx_channel), 1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); +} + +void vtxDecrementChannel() +{ + setChannelSaveAndNotify(&(masterConfig.vtx_channel), -1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX); +} + +void vtxUpdateActivatedChannel() +{ + if (ARMING_FLAG(ARMED)) { + locked = 1; + } + + if (masterConfig.vtx_mode == 2 && !locked) { + static uint8_t lastIndex = -1; + uint8_t index; + + for (index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { + vtxChannelActivationCondition_t *vtxChannelActivationCondition = &masterConfig.vtxChannelActivationConditions[index]; + + if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range) + && index != lastIndex) { + lastIndex = index; + rtc6705SetChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel); + break; + } + } + } +} + +#endif \ No newline at end of file diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h new file mode 100644 index 000000000..ddcd7492d --- /dev/null +++ b/src/main/io/vtx.h @@ -0,0 +1,40 @@ +/* + * 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 . + */ + +#pragma once + +#include "drivers/vtx_rtc6705.h" + +#define VTX_BAND_MIN 1 +#define VTX_BAND_MAX 5 +#define VTX_CHANNEL_MIN 1 +#define VTX_CHANNEL_MAX 8 +#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10 + +typedef struct vtxChannelActivationCondition_s { + uint8_t auxChannelIndex; + uint8_t band; + uint8_t channel; + channelRange_t range; +} vtxChannelActivationCondition_t; + +void vtxInit(); +void vtxIncrementBand(); +void vtxDecrementBand(); +void vtxIncrementChannel(); +void vtxDecrementChannel(); +void vtxUpdateActivatedChannel(); \ No newline at end of file diff --git a/src/main/main.c b/src/main/main.c index 0172dc2eb..d89bb4492 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -70,6 +70,7 @@ #include "io/display.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" +#include "io/vtx.h" #include "sensors/sensors.h" #include "sensors/sonar.h" @@ -397,6 +398,10 @@ void init(void) #endif #endif +#ifdef VTX + vtxInit(); +#endif + #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 54401519b..c5c5e0af5 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -65,7 +65,7 @@ #include "io/statusindicator.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" - +#include "io/vtx.h" #include "rx/rx.h" #include "rx/msp.h" @@ -629,6 +629,9 @@ void processRx(void) } #endif +#ifdef VTX + vtxUpdateActivatedChannel(); +#endif } void subTaskPidController(void) diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 0f2f1d12f..ad184b5aa 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -62,6 +62,7 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" +#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index f9060539b..47685ab0a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -37,6 +37,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/vtx.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" From 0a3bf6e991a4f70d72ad3629893ca8494ae6ec1a Mon Sep 17 00:00:00 2001 From: sblakemore Date: Wed, 2 Dec 2015 08:55:15 +1000 Subject: [PATCH 2/3] Add SINGULARITY target --- Makefile | 20 +- src/main/config/config.c | 15 +- src/main/drivers/pwm_mapping.c | 62 +++ src/main/drivers/timer.c | 25 ++ src/main/sensors/initialisation.c | 13 + .../target/SINGULARITY/system_stm32f30x.c | 372 ++++++++++++++++++ .../target/SINGULARITY/system_stm32f30x.h | 76 ++++ src/main/target/SINGULARITY/target.h | 131 ++++++ 8 files changed, 710 insertions(+), 4 deletions(-) create mode 100644 src/main/target/SINGULARITY/system_stm32f30x.c create mode 100644 src/main/target/SINGULARITY/system_stm32f30x.h create mode 100644 src/main/target/SINGULARITY/target.h diff --git a/Makefile b/Makefile index 58a284d0c..7156073f0 100644 --- a/Makefile +++ b/Makefile @@ -46,7 +46,7 @@ FORKNAME = betaflight CC3D_TARGETS = CC3D CC3D_OPBL -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY # Valid targets for OP VCP support VCP_VALID_TARGETS = $(CC3D_TARGETS) @@ -56,9 +56,9 @@ OPBL_VALID_TARGETS = CC3D_OPBL 64K_TARGETS = CJMCU 128K_TARGETS = ALIENFLIGHTF1 $(CC3D_TARGETS) NAZE OLIMEXINO RMDO AFROMINI -256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE +256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY -F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE +F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY # note that there is no hardfault debugging startup file assembly handler for other platforms ifeq ($(DEBUG_HARDFAULTS),F3) @@ -794,6 +794,20 @@ SPRACINGF3MINI_SRC = \ $(VCP_SRC) # $(FATFS_SRC) +SINGULARITY_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/serial_usb_vcp.c \ + io/flashfs.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) + # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src diff --git a/src/main/config/config.c b/src/main/config/config.c index 7a5d3d153..081cf9e8d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -384,7 +384,7 @@ static void resetConf(void) masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); -#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE) +#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE) || defined(SINGULARITY) featureSet(FEATURE_RX_PPM); #endif @@ -692,6 +692,19 @@ static void resetConf(void) masterConfig.customMotorMixer[7].yaw = -1.0f; #endif + // alternative defaults settings for SINGULARITY target +#if defined(SINGULARITY) + featureSet(FEATURE_BLACKBOX); + masterConfig.blackbox_device = 1; + masterConfig.blackbox_rate_num = 1; + masterConfig.blackbox_rate_denom = 1; + + masterConfig.batteryConfig.vbatscale = 77; + + featureSet(FEATURE_RX_SERIAL); + masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; +#endif + // copy first profile into remaining profile for (i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index b95efed1d..b786ba0ce 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -577,6 +577,62 @@ static const uint16_t airPWM[] = { }; #endif +#if defined(SINGULARITY) +static const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; +#endif + #ifdef SPRACINGF3MINI static const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input @@ -875,6 +931,12 @@ if (init->useBuzzerP6) { if (timerIndex == PWM7 || timerIndex == PWM8) type = MAP_TO_SERVO_OUTPUT; #endif + +#if defined(SINGULARITY) + // remap PWM6+7 as servos + if (timerIndex == PWM6 || timerIndex == PWM7) + type = MAP_TO_SERVO_OUTPUT; +#endif } if (init->useChannelForwarding && !init->airplane) { diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index f0a8ea960..bc52c6e3c 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -391,6 +391,31 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#ifdef SINGULARITY +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, GPIOA, Pin_15, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1}, // PPM/SERIAL RX + + { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // PWM1 + { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // PWM2 + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // PWM3 + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // PWM4 + { TIM16, GPIOB, Pin_8, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_1}, // PWM5 + { TIM17, GPIOB, Pin_9, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_1}, // PWM6 + + { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // SOFTSERIAL1 RX (NC) + { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // SOFTSERIAL1 TX + + { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // LED_STRIP +}; + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + +#endif + #define USED_TIMER_COUNT BITCOUNT(USED_TIMERS) #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 7e05f9688..ab9b27c89 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -182,6 +182,19 @@ const extiConfig_t *selectMPUIntExtiConfig(void) return &MotolabF3MPU6050Config; #endif +#ifdef SINGULARITY + static const extiConfig_t singularityMPU6050Config = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, + .gpioPort = GPIOC, + .gpioPin = Pin_13, + .exti_port_source = EXTI_PortSourceGPIOC, + .exti_pin_source = EXTI_PinSource13, + .exti_line = EXTI_Line13, + .exti_irqn = EXTI15_10_IRQn + }; + return &singularityMPU6050Config; +#endif + #ifdef ALIENFLIGHTF3 // MPU_INT output on V1 PA15 static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = { diff --git a/src/main/target/SINGULARITY/system_stm32f30x.c b/src/main/target/SINGULARITY/system_stm32f30x.c new file mode 100644 index 000000000..fca696982 --- /dev/null +++ b/src/main/target/SINGULARITY/system_stm32f30x.c @@ -0,0 +1,372 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f30x.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Defines + * @{ + */ +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +void SetSysClock(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Enable Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/src/main/target/SINGULARITY/system_stm32f30x.h b/src/main/target/SINGULARITY/system_stm32f30x.h new file mode 100644 index 000000000..4f999d305 --- /dev/null +++ b/src/main/target/SINGULARITY/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h new file mode 100644 index 000000000..052aaa7c2 --- /dev/null +++ b/src/main/target/SINGULARITY/target.h @@ -0,0 +1,131 @@ +/* + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SING" + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_3 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define BEEP_GPIO GPIOC +#define BEEP_PIN Pin_15 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC + +#define USABLE_TIMER_CHANNEL_COUNT 10 + +#define USE_MPU_DATA_READY_SIGNAL + +#define GYRO +#define USE_GYRO_MPU6050 +#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP + +#define ACC +#define USE_ACC_MPU6050 +#define ACC_MPU6050_ALIGN CW0_DEG_FLIP + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define BEEPER +#define LED0 + +#define USE_VCP +#define USE_USART1 // JST-SH Serial - TX (PA9) RX (PA10) +#define USE_USART2 // Input - TX (NC) RX (PA15) +#define USE_USART3 // Solder Pads - TX (PB10) RX (PB11) +#define USE_SOFTSERIAL1 // Telemetry +#define SERIAL_PORT_COUNT 5 + +#define UART1_TX_PIN GPIO_Pin_9 +#define UART1_RX_PIN GPIO_Pin_10 +#define UART1_GPIO GPIOA +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource9 +#define UART1_RX_PINSOURCE GPIO_PinSource10 + +#define UART2_TX_PIN GPIO_Pin_14 //Not connected +#define UART2_RX_PIN GPIO_Pin_15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#define UART3_TX_PIN GPIO_Pin_10 +#define UART3_RX_PIN GPIO_Pin_11 +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 + +#define SOFTSERIAL_1_TIMER TIM15 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 7 //Not connected +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 8 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define M25P16_CS_GPIO GPIOB +#define M25P16_CS_PIN GPIO_Pin_12 +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC2 +#define ADC_DMA_CHANNEL DMA2_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 + +#define VBAT_ADC_GPIO GPIOB +#define VBAT_ADC_GPIO_PIN GPIO_Pin_2 +#define VBAT_ADC_CHANNEL ADC_Channel_12 + +#define LED_STRIP +#define LED_STRIP_TIMER TIM1 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_6 +#define WS2811_PIN GPIO_Pin_8 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM1 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define AUTOTUNE +#define BLACKBOX +#define TELEMETRY +#define SERIAL_RX +#define GPS +#define USE_SERVOS +#define USE_CLI + +#define SPEKTRUM_BIND +// USART2, PA15 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_15 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + From ddee07518943e6c3641302267b2376300908f669 Mon Sep 17 00:00:00 2001 From: sblakemore Date: Fri, 27 May 2016 07:07:19 +1000 Subject: [PATCH 3/3] Add VTX support to SINGULARITY --- Makefile | 2 ++ src/main/target/SINGULARITY/target.h | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/Makefile b/Makefile index 7156073f0..e5a491659 100644 --- a/Makefile +++ b/Makefile @@ -803,7 +803,9 @@ SINGULARITY_SRC = \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_softserial.c \ drivers/serial_usb_vcp.c \ + drivers/vtx_rtc6705.c \ io/flashfs.c \ + io/vtx.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCP_SRC) diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 052aaa7c2..54d182444 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -81,8 +81,14 @@ #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI +#define USE_SPI_DEVICE_1 // PA4, 5, 6, 7 #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#define VTX +#define RTC6705_CS_GPIO GPIOA +#define RTC6705_CS_PIN GPIO_Pin_4 +#define RTC6705_SPI_INSTANCE SPI1 + #define M25P16_CS_GPIO GPIOB #define M25P16_CS_PIN GPIO_Pin_12 #define M25P16_SPI_INSTANCE SPI2