diff --git a/Makefile b/Makefile index f36211887..afae53f0c 100644 --- a/Makefile +++ b/Makefile @@ -61,7 +61,6 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \ drivers/bus_i2c_soft.c \ drivers/gpio_common.c \ drivers/serial_common.c \ - drivers/serial_softserial.c \ drivers/serial_uart.c \ drivers/system_common.c \ flight_imu.c \ @@ -94,12 +93,15 @@ NAZE_SRC = drivers/accgyro_adxl345.c \ drivers/light_ledring.c \ drivers/sonar_hcsr04.c \ drivers/pwm_common.c \ + drivers/serial_softserial.c \ drivers/timer_common.c \ $(COMMON_SRC) # Source files for the FY90Q target -FY90Q_SRC = drivers/adc_fy90q.c \ +FY90Q_SRC = drivers/accgyro_fy90q.c \ + drivers/adc_fy90q.c \ drivers/pwm_fy90q.c \ + drivers/bus_spi.c \ $(COMMON_SRC) # Source files for the OLIMEXINO target @@ -110,6 +112,7 @@ OLIMEXINO_SRC = drivers/accgyro_adxl345.c \ drivers/adc_common.c \ drivers/bus_spi.c \ drivers/pwm_common.c \ + drivers/serial_softserial.c \ drivers/timer_common.c \ $(COMMON_SRC) diff --git a/src/board.h b/src/board.h index 80f1a1af8..88ec985c9 100755 --- a/src/board.h +++ b/src/board.h @@ -233,10 +233,14 @@ typedef struct baro_t #ifdef FY90Q // FY90Q -#include "drivers/adc/drv_adc.h" -#include "drv_i2c.h" -#include "drv_pwm.h" -#include "drivers/serial/drv_uart.h" +#include "drivers/adc_common.h" +#include "drivers/adc_fy90q.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" +#include "drivers/pwm_common.h" +#include "drivers/timer_common.h" +#include "drivers/serial_common.h" +#include "drivers/serial_uart.h" #else #ifdef OLIMEXINO diff --git a/src/drivers/accgyro_fy90q.c b/src/drivers/accgyro_fy90q.c new file mode 100644 index 000000000..5a344e69e --- /dev/null +++ b/src/drivers/accgyro_fy90q.c @@ -0,0 +1,72 @@ +#ifdef FY90Q + +#include +#include + +#include "platform.h" + +#include "accgyro_common.h" +#include "sensors_common.h" + +#include "system_common.h" + +#include "adc_fy90q.h" + +//#include "boardalignment.h" + +extern uint16_t acc_1G; + +static void adcAccRead(int16_t *accelData); +static void adcGyroRead(int16_t *gyroData); +static void adcDummyInit(sensor_align_e align); + +void adcSensorInit(sensor_t *acc, sensor_t *gyro) +{ + acc->init = adcDummyInit; + acc->read = adcAccRead; + + gyro->init = adcDummyInit; + gyro->read = adcGyroRead; + gyro->scale = 1.0f; + + acc_1G = 376; +} + +static void adcAccRead(int16_t *accelData) +{ + // ADXL335 + // 300mV/g + // Vcc 3.0V + accelData[0] = adcData[3]; + accelData[1] = adcData[4]; + accelData[2] = adcData[5]; +} + +static void adcGyroRead(int16_t *gyroData) +{ + // Vcc: 3.0V + // Pitch/Roll: LPR550AL, 2000dps mode. + // 0.5mV/dps + // Zero-rate: 1.23V + // Yaw: LPY550AL, 2000dps mode. + // 0.5mV/dps + // Zero-rate: 1.23V + + // Need to match with: 14.375lsb per dps + // 12-bit ADC + + gyroData[0] = adcData[0] * 2; + gyroData[1] = adcData[1] * 2; + gyroData[2] = adcData[2] * 2; +} + +static void adcDummyInit(sensor_align_e align) +{ + // nothing to init here +} + +uint16_t adcGetBattery(void) +{ + return 0; +} +#endif diff --git a/src/drivers/adc_common.c b/src/drivers/adc_common.c index d27f691c5..4fd809812 100755 --- a/src/drivers/adc_common.c +++ b/src/drivers/adc_common.c @@ -5,10 +5,7 @@ #include "accgyro_common.h" -#include "board.h" - -#define ADC_BATTERY 0 -#define ADC_CURRENT 1 +#include "adc_common.h" // static volatile uint16_t adc1Ch4Value = 0; static volatile uint16_t adcValues[2]; diff --git a/src/drivers/adc_fy90q.c b/src/drivers/adc_fy90q.c index 07689ce0e..f874c87f4 100644 --- a/src/drivers/adc_fy90q.c +++ b/src/drivers/adc_fy90q.c @@ -1,30 +1,20 @@ #ifdef FY90Q -#include "board.h" -#define ADC_CHANNELS 9 +#include +#include + +#include "platform.h" + +#include "accgyro_common.h" +#include "sensors_common.h" + +#include "system_common.h" + +#include "adc_fy90q.h" + +//#include "boardalignment.h" volatile uint16_t adcData[ADC_CHANNELS] = { 0, }; -extern uint16_t acc_1G; - -static void adcAccRead(int16_t *accelData); -static void adcAccAlign(int16_t *accelData); -static void adcGyroRead(int16_t *gyroData); -static void adcGyroAlign(int16_t *gyroData); -static void adcDummyInit(void); - -void adcSensorInit(sensor_t *acc, sensor_t *gyro) -{ - acc->init = adcDummyInit; - acc->read = adcAccRead; - acc->align = adcAccAlign; - - gyro->init = adcDummyInit; - gyro->read = adcGyroRead; - gyro->align = adcGyroAlign; - gyro->scale = 1.0f; - - acc_1G = 376; -} void adcCalibrateADC(ADC_TypeDef *ADCx, int n) { @@ -95,51 +85,9 @@ void adcInit(void) ADC_SoftwareStartConvCmd(ADC1, ENABLE); } -static void adcAccRead(int16_t *accelData) +uint16_t adcGetChannel(uint8_t channel) { - // ADXL335 - // 300mV/g - // Vcc 3.0V - accelData[0] = adcData[3]; - accelData[1] = adcData[4]; - accelData[2] = adcData[5]; + return 0; // Not Supported } -static void adcAccAlign(int16_t *accelData) -{ - // align OK -} - -static void adcGyroRead(int16_t *gyroData) -{ - // Vcc: 3.0V - // Pitch/Roll: LPR550AL, 2000dps mode. - // 0.5mV/dps - // Zero-rate: 1.23V - // Yaw: LPY550AL, 2000dps mode. - // 0.5mV/dps - // Zero-rate: 1.23V - - // Need to match with: 14.375lsb per dps - // 12-bit ADC - - gyroData[0] = adcData[0] * 2; - gyroData[1] = adcData[1] * 2; - gyroData[2] = adcData[2] * 2; -} - -static void adcGyroAlign(int16_t *gyroData) -{ - // align OK -} - -static void adcDummyInit(void) -{ - // nothing to init here -} - -uint16_t adcGetBattery(void) -{ - return 0; -} #endif diff --git a/src/drivers/adc_fy90q.h b/src/drivers/adc_fy90q.h new file mode 100644 index 000000000..7b3ee73ff --- /dev/null +++ b/src/drivers/adc_fy90q.h @@ -0,0 +1,5 @@ +#pragma once + +#define ADC_CHANNELS 9 + +extern volatile uint16_t adcData[ADC_CHANNELS]; diff --git a/src/drivers/pwm_fy90q.c b/src/drivers/pwm_fy90q.c index cb33e6b8e..abc48629a 100644 --- a/src/drivers/pwm_fy90q.c +++ b/src/drivers/pwm_fy90q.c @@ -1,5 +1,14 @@ #ifdef FY90Q -#include "board.h" + +#include "stdbool.h" +#include "stdint.h" +#include "platform.h" + +#include "serial_common.h" + +#include "pwm_common.h" + +static uint16_t failsafeThreshold = 985; #define PULSE_1MS (1000) // 1ms pulse width // #define PULSE_PERIOD (2500) // pulse period (400Hz) @@ -71,7 +80,6 @@ static void ppmIRQHandler(TIM_TypeDef *tim) if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) { last = now; now = TIM_GetCapture1(tim); - rcActive = true; } TIM_ClearITPendingBit(tim, TIM_IT_CC1); @@ -87,7 +95,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim) } else { if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range Inputs[chan].capture = diff; - if (chan < 4 && diff > FAILSAFE_DETECT_TRESHOLD) + if (chan < 4 && diff > failsafeThreshold) GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK if (GoodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter GoodPulses = 0; @@ -203,7 +211,7 @@ static void pwmInitializeInput(bool usePPM) TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE); TIM_Cmd(TIM2, ENABLE); - // configure number of PWM outputs, in PPM mode, we use bottom 4 channels more more motors + // configure number of PWM outputs, in PPM mode, we use bottom 4 channels for more motors numOutputChannels = 10; } else { // Configure TIM2 all 4 channels @@ -331,12 +339,17 @@ bool pwmInit(drv_pwm_config_t *init) return false; } -void pwmWrite(uint8_t channel, uint16_t value) +void pwmWriteMotor(uint8_t channel, uint16_t value) { if (channel < numOutputChannels) *OutputChannels[channel] = value; } +void pwmWriteServo(uint8_t channel, uint16_t value) +{ + // Not supported +} + uint16_t pwmRead(uint8_t channel) { return Inputs[channel].capture; diff --git a/src/main.c b/src/main.c index 58f60eb09..91bf8b76f 100755 --- a/src/main.c +++ b/src/main.c @@ -148,6 +148,7 @@ int main(void) serialInit(mcfg.serial_baudrate); +#ifndef FY90Q if (feature(FEATURE_SOFTSERIAL)) { //mcfg.softserial_baudrate = 19200; // Uncomment to override config value @@ -163,6 +164,7 @@ int main(void) #endif //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial. } +#endif if (feature(FEATURE_TELEMETRY)) initTelemetry(); diff --git a/src/serial_cli.c b/src/serial_cli.c index 6503d1fea..40f84ec7b 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -58,7 +58,11 @@ static const char * const mixerNames[] = { // sync this with AvailableFeatures enum from board.h static const char * const featureNames[] = { "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP", - "SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", + "SERVO_TILT", +#ifndef FY90Q + "SOFTSERIAL", +#endif + "LED_RING", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", NULL }; diff --git a/src/telemetry_common.c b/src/telemetry_common.c index 6302a1855..880d15d6e 100644 --- a/src/telemetry_common.c +++ b/src/telemetry_common.c @@ -46,14 +46,22 @@ void initTelemetry(void) if (!feature(FEATURE_SOFTSERIAL)) mcfg.telemetry_port = TELEMETRY_PORT_UART; +#ifdef FY90Q + // FY90Q does not support softserial + mcfg.telemetry_port = TELEMETRY_PORT_UART; + core.telemport = core.mainport; +#endif + isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration(); +#ifndef FY90Q if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1) core.telemport = &(softSerialPorts[0].port); else if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) core.telemport = &(softSerialPorts[1].port); else core.telemport = core.mainport; +#endif checkTelemetryState(); }