diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index b4968ec212..e690f9c040 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -68,7 +68,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER", - "SOFTSPI", "NRF24" + "SOFTSPI", "NRF24", "VTX" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index f4a85912fa..52ce552eae 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -32,6 +32,7 @@ typedef enum { OWNER_TX, OWNER_SOFTSPI, OWNER_RX_SPI, + OWNER_VTX, OWNER_TOTAL_COUNT } resourceOwner_t; diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c new file mode 100644 index 0000000000..992158a75f --- /dev/null +++ b/src/main/drivers/vtx_rtc6705.c @@ -0,0 +1,265 @@ +/* + * 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 "platform.h" + +#if defined(VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI) + +#include "common/maths.h" + +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "drivers/vtx_rtc6705.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) + +#ifdef RTC6705_POWER_PIN +static IO_t vtxPowerPin = IO_NONE; +#endif +static IO_t vtxCSPin = IO_NONE; + +#define DISABLE_RTC6705() IOHi(vtxCSPin) + +#ifdef USE_RTC6705_CLK_HACK +static IO_t vtxCLKPin = IO_NONE; +// HACK for missing pull up on CLK line - drive the CLK high *before* enabling the CS pin. +#define ENABLE_RTC6705() {IOHi(vtxCLKPin); delayMicroseconds(5); IOLo(vtxCSPin); } +#else +#define ENABLE_RTC6705() IOLo(vtxCSPin) +#endif + +#define DP_5G_MASK 0x7000 // b111000000000000 +#define PA5G_BS_MASK 0x0E00 // b000111000000000 +#define PA5G_PW_MASK 0x0180 // b000000110000000 +#define PD_Q5G_MASK 0x0040 // b000000001000000 +#define QI_5G_MASK 0x0038 // b000000000111000 +#define PA_BS_MASK 0x0007 // b000000000000111 + +#define PA_CONTROL_DEFAULT 0x4FBD + +#define RTC6705_RW_CONTROL_BIT (1 << 4) +#define RTC6705_ADDRESS (0x07) + +#define ENABLE_VTX_POWER() IOLo(vtxPowerPin) +#define DISABLE_VTX_POWER() IOHi(vtxPowerPin) + + +// Define variables +static const uint32_t channelArray[RTC6705_BAND_COUNT][RTC6705_CHANNEL_COUNT] = { + { 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 }, +}; + +/** + * 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 + */ + +void rtc6705IOInit(void) +{ +#ifdef RTC6705_POWER_PIN + + vtxPowerPin = IOGetByTag(IO_TAG(RTC6705_POWER_PIN)); + IOInit(vtxPowerPin, OWNER_VTX, RESOURCE_OUTPUT, 0); + + DISABLE_VTX_POWER(); + IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP); +#endif + +#ifdef USE_RTC6705_CLK_HACK + vtxCLKPin = IOGetByTag(IO_TAG(RTC6705_CLK_PIN)); + // we assume the CLK pin will have been initialised by the SPI code. +#endif + + vtxCSPin = IOGetByTag(IO_TAG(RTC6705_CS_PIN)); + IOInit(vtxCSPin, OWNER_VTX, RESOURCE_OUTPUT, 0); + + DISABLE_RTC6705(); + // GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode. + // Note: It's critical to ensure that incorrect signals are not sent to the VTX. + IOConfigGPIO(vtxCSPin, IOCFG_OUT_PP); +} + +/** + * 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); + + delayMicroseconds(2); + + DISABLE_RTC6705(); + + delayMicroseconds(2); +} + +/** + * Set a band and channel + */ +void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel) +{ + band = constrain(band, 0, RTC6705_BAND_COUNT - 1); + channel = constrain(channel, 0, RTC6705_CHANNEL_COUNT - 1); + + spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); + + rtc6705Transfer(RTC6705_SET_HEAD); + rtc6705Transfer(channelArray[band][channel]); +} + + /** + * Set a freq in mhz + * Formula derived from datasheet + */ +void rtc6705SetFreq(uint16_t frequency) +{ + frequency = constrain(frequency, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX); + + uint32_t val_hex = 0; + + uint32_t val_a = ((((uint64_t)frequency*(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)frequency*(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); + + spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); + + rtc6705Transfer(RTC6705_SET_HEAD); + delayMicroseconds(10); + rtc6705Transfer(val_hex); +} + +void rtc6705SetRFPower(uint8_t rf_power) +{ + rf_power = constrain(rf_power, 0, RTC6705_RF_POWER_COUNT - 1); + + spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); + + uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write + val_hex |= RTC6705_ADDRESS; // address + uint32_t data = rf_power == 0 ? (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)) : PA_CONTROL_DEFAULT; + val_hex |= data << 5; // 4 address bits and 1 rw bit. + + rtc6705Transfer(val_hex); +} + +void rtc6705Disable(void) +{ +#ifdef RTC6705_POWER_PIN + DISABLE_VTX_POWER(); +#endif +} + +void rtc6705Enable(void) +{ +#ifdef RTC6705_POWER_PIN + ENABLE_VTX_POWER(); +#endif +} + +#endif diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h new file mode 100644 index 0000000000..3baa594573 --- /dev/null +++ b/src/main/drivers/vtx_rtc6705.h @@ -0,0 +1,43 @@ +/* + * 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_COUNT 5 +#define RTC6705_CHANNEL_COUNT 8 +#define RTC6705_RF_POWER_COUNT 2 + +#define RTC6705_FREQ_MIN 5600 +#define RTC6705_FREQ_MAX 5950 + +#define RTC6705_BOOT_DELAY 350 // milliseconds + +void rtc6705IOInit(void); +void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel); +void rtc6705SetFreq(const uint16_t freq); +void rtc6705SetRFPower(const uint8_t rf_power); +void rtc6705Disable(void); +void rtc6705Enable(void); diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c new file mode 100644 index 0000000000..0acd4e683e --- /dev/null +++ b/src/main/drivers/vtx_rtc6705_soft_spi.c @@ -0,0 +1,156 @@ +/* + * 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 "platform.h" + +#if defined(VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI) + +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/system.h" +#include "light_led.h" + +#include "vtx_rtc6705.h" + +#define DP_5G_MASK 0x7000 +#define PA5G_BS_MASK 0x0E00 +#define PA5G_PW_MASK 0x0180 +#define PD_Q5G_MASK 0x0040 +#define QI_5G_MASK 0x0038 +#define PA_BS_MASK 0x0007 + +#define PA_CONTROL_DEFAULT 0x4FBD + +#define RTC6705_SPICLK_ON IOHi(rtc6705ClkPin) +#define RTC6705_SPICLK_OFF IOLo(rtc6705ClkPin) + +#define RTC6705_SPIDATA_ON IOHi(rtc6705DataPin) +#define RTC6705_SPIDATA_OFF IOLo(rtc6705DataPin) + +#define RTC6705_SPILE_ON IOHi(rtc6705LePin) +#define RTC6705_SPILE_OFF IOLo(rtc6705LePin) + +const uint16_t vtx_freq[] = +{ + 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // Boacam A + 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // Boscam B + 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // Boscam E + 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // FatShark + 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // RaceBand +}; + +static IO_t rtc6705DataPin = IO_NONE; +static IO_t rtc6705LePin = IO_NONE; +static IO_t rtc6705ClkPin = IO_NONE; + +void rtc6705IOInit(void) +{ + rtc6705DataPin = IOGetByTag(IO_TAG(RTC6705_SPIDATA_PIN)); + rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN)); + rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN)); + + IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET); + IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); + + IOInit(rtc6705LePin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET); + IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP); + + IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET); + IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP); +} + +static void rtc6705_write_register(uint8_t addr, uint32_t data) +{ + uint8_t i; + + RTC6705_SPILE_OFF; + delay(1); + // send address + for (i=0; i<4; i++) { + if ((addr >> i) & 1) + RTC6705_SPIDATA_ON; + else + RTC6705_SPIDATA_OFF; + + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + } + // Write bit + + RTC6705_SPIDATA_ON; + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + for (i=0; i<20; i++) { + if ((data >> i) & 1) + RTC6705_SPIDATA_ON; + else + RTC6705_SPIDATA_OFF; + RTC6705_SPICLK_ON; + delay(1); + RTC6705_SPICLK_OFF; + delay(1); + } + RTC6705_SPILE_ON; +} + +void rtc6705SetFreq(uint16_t channel_freq) +{ + uint32_t freq = (uint32_t)channel_freq * 1000; + uint32_t N, A; + + freq /= 40; + N = freq / 64; + A = freq % 64; + rtc6705_write_register(0, 400); + rtc6705_write_register(1, (N << 7) | A); +} + +void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel) +{ + // band and channel are 1-based, not 0-based + + // example for raceband/ch8: + // (5 - 1) * 8 + (8 - 1) + // 4 * 8 + 7 + // 32 + 7 = 39 + uint8_t freqIndex = ((band - 1) * RTC6705_BAND_COUNT) + (channel - 1); + + uint16_t freq = vtx_freq[freqIndex]; + rtc6705SetFreq(freq); +} + +void rtc6705SetRFPower(const uint8_t rf_power) +{ + rtc6705_write_register(7, (rf_power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)))); +} + +void rtc6705Disable(void) +{ +} + +void rtc6705Enable(void) +{ +} + +#endif diff --git a/src/main/io/serial.c b/src/main/io/serial.c index c5701a1ae7..a01483dab6 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -155,6 +155,16 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate) return BAUD_AUTO; } +int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier) +{ + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + if (serialPortIdentifiers[index] == identifier) { + return index; + } + } + return -1; +} + serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier) { uint8_t index; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index b4f158a4ae..989cd7d8b2 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -134,7 +134,7 @@ portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialP bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier); - +int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier); // // runtime // diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 62ce989b4b..42e585786d 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -17,6 +17,7 @@ #pragma once +#include "common/axis.h" #include "common/time.h" #include "config/parameter_group.h" @@ -25,6 +26,7 @@ #include "sensors/sensors.h" + // Type of magnetometer used/detected typedef enum { MAG_NONE = 0, diff --git a/src/main/target/SPRACINGF3NEO/config.c b/src/main/target/SPRACINGF3NEO/config.c new file mode 100644 index 0000000000..50ddd2be1d --- /dev/null +++ b/src/main/target/SPRACINGF3NEO/config.c @@ -0,0 +1,46 @@ +/* + * 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 "platform.h" + +#ifdef TARGET_CONFIG + +#include "io/serial.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/compass.h" +#include "sensors/barometer.h" + +#include "telemetry/telemetry.h" + +void targetConfiguration(void) +{ + barometerConfigMutable()->baro_hardware = BARO_AUTODETECT; + compassConfigMutable()->mag_hardware = MAG_AUTODETECT; + rxConfigMutable()->sbus_inversion = 1; + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So Bluetooth users don't have to change anything. + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS; + //telemetryConfigMutable()->telemetry_inversion = 1; + //telemetryConfigMutable()->halfDuplex = 1; +} +#endif diff --git a/src/main/target/SPRACINGF3NEO/target.c b/src/main/target/SPRACINGF3NEO/target.c new file mode 100755 index 0000000000..b4b97e0ba4 --- /dev/null +++ b/src/main/target/SPRACINGF3NEO/target.c @@ -0,0 +1,119 @@ +/* + * 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 "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + 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), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +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), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + 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), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + 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), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM1 / PPM / UART2 RX + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM2 + + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC1 + +#ifdef USE_DSHOT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC2 +#else + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC2 +#endif + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC3 + +#ifdef USE_DSHOT + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC4 +#else + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC4 +#endif + +#ifndef USE_DSHOT + // with DSHOT TIM8 is used for DSHOT and cannot be used for PWM + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC5 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // ESC6 +#endif + + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM3 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM4 - PB11 - *TIM2_CH4, UART3_RX (AF7) + + // with DSHOT DMA1-CH3 conflicts with TIM3_CH4 / ESC1. + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 } +}; + diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h new file mode 100755 index 0000000000..75e0aa29de --- /dev/null +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -0,0 +1,198 @@ +/* + * 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 "SP3N" +#define TARGET_CONFIG + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define LED0 PB9 +#define LED1 PB2 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 4 // MPU_INT, SDCardDetect, OSD +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define GYRO +#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_SPI_MPU6500 + +#define ACC_MPU6500_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG + +#define BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define MAG +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +//#define USE_SOFTSERIAL1 +//#define USE_SOFTSERIAL2 +//#define SERIAL_PORT_COUNT 8 +#define SERIAL_PORT_COUNT 6 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) + +#define USE_SPI +#define USE_SPI_DEVICE_1 // MPU +#define USE_SPI_DEVICE_2 // SDCard +#define USE_SPI_DEVICE_3 // External (MAX7456 & RTC6705) + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define VTX_RTC6705 +#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL + +#undef VTX_SMARTAUDIO // Disabled due to flash size +#undef VTX_TRAMP // Disabled due to flash size + +#define RTC6705_CS_PIN PF4 +#define RTC6705_SPI_INSTANCE SPI3 +#define RTC6705_POWER_PIN PC3 + +#define USE_RTC6705_CLK_HACK +#define RTC6705_CLK_PIN SPI3_SCK_PIN + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 + +#define MAX7456_DMA_CHANNEL_TX DMA2_Channel2 +#define MAX7456_DMA_CHANNEL_RX DMA2_Channel1 +#define MAX7456_DMA_IRQ_HANDLER_ID DMA2_CH1_HANDLER + +#define SPI_SHARED_MAX7456_AND_RTC6705 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_INSTANCE SPI1 + +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER 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 WS2811_TIMER_GPIO_AF GPIO_AF_6 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define OSD + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP) +#define SERIALRX_UART SERIAL_PORT_USART2 +#define GPS_UART SERIAL_PORT_USART3 +#define TELEMETRY_UART SERIAL_PORT_USART5 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define BUTTONS +#define BUTTON_A_PIN PD2 + +#define SPEKTRUM_BIND_PIN UART2_RX_PIN + +#define BINDPLUG_PIN PD2 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define MAX_PWM_OUTPUT_PORTS 10 + +// IO - assuming 303 in 64pin package, TODO +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 12 // 2xPPM, 6xPWM, UART3 RX/TX, LED Strip, IR. +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16)) diff --git a/src/main/target/SPRACINGF3NEO/target.mk b/src/main/target/SPRACINGF3NEO/target.mk new file mode 100755 index 0000000000..d347b78958 --- /dev/null +++ b/src/main/target/SPRACINGF3NEO/target.mk @@ -0,0 +1,13 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_ak8975.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/max7456.c \ + drivers/vtx_rtc6705.c