diff --git a/make.py b/make.py index 644a1019..0663492c 100755 --- a/make.py +++ b/make.py @@ -577,6 +577,7 @@ def configure_target(TARGET): INCLUDES = " ".join("-I" + include for include in INCLUDE_DIRS) LTO_FLAGS = "-flto -fuse-linker-plugin" + #DEBUG_FLAGS = "-ggdb3 -DDEBUG -O0" DEBUG_FLAGS = "-ggdb3 -DDEBUG -Og" CFLAGS = " ".join([ diff --git a/src/flight_controller/inc/telemetry/dshotTwoWay.h b/src/flight_controller/inc/telemetry/dshotTwoWay.h index 3d608c95..79fe9ab3 100644 --- a/src/flight_controller/inc/telemetry/dshotTwoWay.h +++ b/src/flight_controller/inc/telemetry/dshotTwoWay.h @@ -51,6 +51,7 @@ typedef struct uint32_t motorCommMask; } dshot_command_handler; +extern volatile int dShotFeedTheDog; extern volatile dshot_command_handler dshotCommandHandler; extern void DshotInit(int offlineMode); @@ -58,3 +59,4 @@ extern int InitDshotCommandState(void); extern int HandleDshotCommands(void); extern void ThrottleToDshot(uint8_t *serialOutBuffer, float throttle, float idle, int reverse); extern void CommandToDshot(uint8_t *serialOutBuffer, uint16_t command); +extern void DshotTransferComplete(uint32_t callbackNumber); \ No newline at end of file diff --git a/src/flight_controller/src/config.c b/src/flight_controller/src/config.c index ee014732..494a741a 100644 --- a/src/flight_controller/src/config.c +++ b/src/flight_controller/src/config.c @@ -1,6 +1,5 @@ #include "includes.h" - // use variable record but instead of storing address of variable, store offset based on address of field, that way it works with the record loaded from file extern volatile int logMe; diff --git a/src/flight_controller/src/imu.c b/src/flight_controller/src/imu.c index 4ad1c704..128e829f 100644 --- a/src/flight_controller/src/imu.c +++ b/src/flight_controller/src/imu.c @@ -40,6 +40,11 @@ quaternion_record MultiplyQuatAndQuat(volatile quaternion_record quatIn1, volati void EulerToVector(volatile vector_record *outVector, float x, float y, float z); void VectorAddVector(volatile vector_record *vectorOut, vector_record vectorIn, float trust); +vector_record RotateVectorByQuaternionQV(volatile quaternion_record quatIn, volatile vector_record vectorIn); +void VectorCrossProduct(volatile vector_record *vectorOut, volatile vector_record vectorIn1, volatile vector_record vectorIn2); +vector_record RotateVectorByQuaternionVQ(volatile vector_record vectorIn, volatile quaternion_record quatIn); +vector_record QuaternionToVector(volatile quaternion_record quatIn); + //quats are defined like this: X is roll, Y is pitch, Z is yaw. //Positive X is a roll to the right which matches our gyro //Positive Y is a pitch down which is opposite of our gyro diff --git a/src/flight_controller/src/main.c b/src/flight_controller/src/main.c index 9296c79b..955ee492 100644 --- a/src/flight_controller/src/main.c +++ b/src/flight_controller/src/main.c @@ -19,10 +19,12 @@ int main(void) retValChk = BoardInit(); - //DshotInit(1); - retValChk = HandleRfbl(); + DshotInit(1); + //TESTING ONLY!! + while(1); + retValChk = LoadConfig(ADDRESS_CONFIG_START); SpektrumBind(mainConfig.rcControlsConfig.bind); diff --git a/src/flight_controller/src/scheduler.c b/src/flight_controller/src/scheduler.c index e74ab726..06fa2471 100644 --- a/src/flight_controller/src/scheduler.c +++ b/src/flight_controller/src/scheduler.c @@ -436,7 +436,7 @@ void TaskAdc(void) void TaskProcessSoftSerial(void) { //feed the dog if oneWire is active to prevent WD restart - if (oneWireActive) + if (oneWireActive || dShotFeedTheDog) FeedTheDog(); //feed the dog if dshot commanding is active to prevent WD restart diff --git a/src/flight_controller/src/telemetry/dshotTwoWay.c b/src/flight_controller/src/telemetry/dshotTwoWay.c index e8ae7596..916184d0 100644 --- a/src/flight_controller/src/telemetry/dshotTwoWay.c +++ b/src/flight_controller/src/telemetry/dshotTwoWay.c @@ -3,6 +3,7 @@ #define DSHOT_OUT_BUFFER_SIZE 48 //16 * 3 bytes uint32_t dshotOutBuffer[DSHOT_OUT_BUFFER_SIZE]; uint32_t dshotInBuffer[DSHOT_OUT_BUFFER_SIZE*8]; +volatile int dShotFeedTheDog; //static void DshotTimerInit(uint32_t timer, uint32_t pwmHz, uint32_t timerHz); //make general function? @@ -188,7 +189,131 @@ void CommandToDshot(uint8_t *serialOutBuffer, uint16_t command) ///////////////////////////GPIO Manipulation method: -/* +static void DshotDmaInit(void) //make general function? +{ + //init DMA + //for testing we hard code DMA2 Stream 5 + dmaHandles[ENUM_DMA2_STREAM_5].Instance = DMA2_Stream5; + dmaHandles[ENUM_DMA2_STREAM_5].Init.Channel = DMA_CHANNEL_6; + dmaHandles[ENUM_DMA2_STREAM_5].Init.Direction = DMA_MEMORY_TO_PERIPH; + dmaHandles[ENUM_DMA2_STREAM_5].Init.PeriphInc = DMA_PINC_DISABLE; + dmaHandles[ENUM_DMA2_STREAM_5].Init.MemInc = DMA_MINC_ENABLE; + dmaHandles[ENUM_DMA2_STREAM_5].Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + dmaHandles[ENUM_DMA2_STREAM_5].Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + dmaHandles[ENUM_DMA2_STREAM_5].Init.Mode = DMA_NORMAL; + dmaHandles[ENUM_DMA2_STREAM_5].Init.Priority = DMA_PRIORITY_HIGH; + dmaHandles[ENUM_DMA2_STREAM_5].Init.FIFOMode = DMA_FIFOMODE_DISABLE; + + HAL_DMA_UnRegisterCallback(&dmaHandles[ENUM_DMA2_STREAM_5], HAL_DMA_XFER_ALL_CB_ID); + + //only need the first one of these? + __HAL_LINKDMA(&dshotTimer,hdma[TIM_DMA_ID_UPDATE],dmaHandles[ENUM_DMA2_STREAM_5]); + __HAL_LINKDMA(&dshotTimer,hdma[TIM_DMA_ID_CC3],dmaHandles[ENUM_DMA2_STREAM_5]); + + if (HAL_DMA_Init(&dmaHandles[ENUM_DMA2_STREAM_5]) != HAL_OK) + { + while(1); + } + + HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 0, 2); + HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn); + + //register the callback function + callbackFunctionArray[FP_DMA2_S5] = DshotTransferComplete; +} + +static void DshotTimerInit(uint32_t timer, uint32_t pwmHz, uint32_t timerHz) //make general function? +{ + + TIM_ClockConfigTypeDef sClockSourceConfig; //set, timer variable + TIM_MasterConfigTypeDef sMasterConfig; //set, timer variable + + (void)(pwmHz); + //run at 24 MHz, need a bitrate of 3 bits per byte: + //three types of "bytes" + //one, zero, nothing: + //one is XXx. + //zero is Xxx, + //nothing is xxx, + //Bit length (total timing period) is 1.67 microseconds + //For a bit to be 0, the pulse width is 625 nanoseconds + //For a bit to be 1, the pulse width is 1250 nanoseconds + //For DSHOT 600: + //15 and 24000000 MHz will generate a timer event ever 625 ns when the timer is active + //(15 / 24,000,000) = 0.000,000,625 seconds + //3 timer events per bit, 000 = nothing, 100 = zero, 110 = one + + dshotTimer.Instance = timers[timer]; + dshotTimer.Init.Prescaler = (uint16_t)(SystemCoreClock / TimerPrescalerDivisor(timer) / timerHz) - 1; + //dshotTimer.Init.CounterMode = TIM_COUNTERMODE_UP; + //dshotTimer.Init.Period = (timerHz / pwmHz) - 1;; + //dshotTimer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + //dshotTimer.Init.Prescaler = 1024; + dshotTimer.Init.CounterMode = TIM_COUNTERMODE_UP; + dshotTimer.Init.Period = 3; + dshotTimer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + if (HAL_TIM_Base_Init(&dshotTimer) != HAL_OK) + { + while(1); + } + + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&dshotTimer, &sClockSourceConfig) != HAL_OK) + { + while(1); + } + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&dshotTimer, &sMasterConfig) != HAL_OK) + { + while(1); + } + +} + +uint16_t number = 0; + +void SuperFillDshotBuffer(void) +{ + uint32_t motorMask; + + #define DSHOT_MOTOR_PORT GPIOB + #define DSHOT_MOTOR_1_PIN GPIO_PIN_0 + #define DSHOT_MOTOR_2_PIN GPIO_PIN_1 + #define DSHOT_MOTOR_3_PIN GPIO_PIN_2 + #define DSHOT_MOTOR_4_PIN GPIO_PIN_3 + + motorMask = (DSHOT_MOTOR_1_PIN | DSHOT_MOTOR_2_PIN | DSHOT_MOTOR_3_PIN | DSHOT_MOTOR_4_PIN); + + int dshotBit; + + memset(dshotOutBuffer, 0, sizeof(dshotOutBuffer)); + + //signal is 0 to all motors: + for(int x=0;x<48;x+=3) + { + + dshotBit = (number & (1 << (x/3))) ? 1 : 0; //load data into framedata one bit at a time + + if(dshotBit) + { + //set a one + dshotOutBuffer[x] |= motorMask; + dshotOutBuffer[x+1] |= motorMask; + dshotOutBuffer[x+2] |= (motorMask << 16); + } + else + { + //set a zero + dshotOutBuffer[x] |= motorMask; + dshotOutBuffer[x+1] |= (motorMask << 16); + dshotOutBuffer[x+2] |= (motorMask << 16); + } + } + number++; +} + void FillDshotBuffer(uint16_t data, uint32_t gpio) { int bitIndex; @@ -216,52 +341,38 @@ void FillDshotBuffer(uint16_t data, uint32_t gpio) void DshotInit(int offlineMode) { - //todo change from single buffer method maybe - //int outputNumber; //set, motor output based on board orrientation, from quad's POV: 0 is top left, 1 is top right, etc... - int motorNum; //set, motor output based on board arrat, from board's POV: 0 is top left, 1 is top right, etc... + + #define DSHOT_MOTOR_PORT GPIOB + #define DSHOT_MOTOR_1_PIN GPIO_PIN_0 + #define DSHOT_MOTOR_2_PIN GPIO_PIN_1 + #define DSHOT_MOTOR_3_PIN GPIO_PIN_2 + #define DSHOT_MOTOR_4_PIN GPIO_PIN_3 //deinit flight and treat motors independantly, make this compatible with old board if(offlineMode) { dShotFeedTheDog = 1; //feed the dog in the scheduler - //DeinitFlight(); //disable gyro, acc, motor outputs, serial, and soft serial + DeinitFlight(); //disable gyro, acc, motor outputs, serial, and soft serial } + SuperFillDshotBuffer(); + + //single port + InitializeGpio(DSHOT_MOTOR_PORT, DSHOT_MOTOR_1_PIN, 0); + InitializeGpio(DSHOT_MOTOR_PORT, DSHOT_MOTOR_2_PIN, 0); + InitializeGpio(DSHOT_MOTOR_PORT, DSHOT_MOTOR_3_PIN, 0); + InitializeGpio(DSHOT_MOTOR_PORT, DSHOT_MOTOR_4_PIN, 0); + //init timer DshotTimerInit(ENUM_TIM1, 15, 24000000); - //DshotTimerInit(ENUM_TIM1, 512, 1024); //slow way down for visual test DshotDmaInit(); + if ((HAL_TIM_Base_Start(&dshotTimer)) != HAL_OK) { while(1); } - - bzero(dshotOutBuffer, sizeof(dshotOutBuffer)); - for(motorNum=0;motorNumODR, DSHOT_OUT_BUFFER_SIZE) != HAL_OK) - // { - // while(1); - // } - //} - - } - InitializeGpio(ports[ENUM_PORTA], GPIO_PIN_2, 1); //use B5 to test output on LED - FillDshotBuffer(0xAAAA, GPIO_PIN_2); - //if (HAL_DMA_Start_IT(&dmaHandles[ENUM_DMA2_STREAM_5], (uint32_t)&dshotOutBuffer, (uint32_t)&GPIOA->ODR, DSHOT_OUT_BUFFER_SIZE) != HAL_OK) - //{ - // while(1); - //} - if (HAL_DMA_Start_IT(&dmaHandles[ENUM_DMA2_STREAM_5], (uint32_t)&dshotOutBuffer, (uint32_t)&GPIOA->ODR, DSHOT_OUT_BUFFER_SIZE) != HAL_OK) + + if (HAL_DMA_Start_IT(&dmaHandles[ENUM_DMA2_STREAM_5], (uint32_t)&dshotOutBuffer, (uint32_t)&DSHOT_MOTOR_PORT->BSRR, DSHOT_OUT_BUFFER_SIZE) != HAL_OK) { while(1); } @@ -291,14 +402,16 @@ void DshotInit(int offlineMode) DelayMs(6000); } -static void DshotTransferComplete(uint32_t callbackNumber) +void DshotTransferComplete(uint32_t callbackNumber) { - static int fisherton = 0; + //static int fisherton = 0; (void)(callbackNumber); TIM1->DIER &= ~(0x00000100); volatile int transferCompleteDetected = 1; //1.55us from completion to this beingtriggered - //inlineDigitalHi(ports[ENUM_PORTA], GPIO_PIN_2); + + SuperFillDshotBuffer(); + /* if(fisherton) { fisherton = 0; @@ -309,98 +422,21 @@ static void DshotTransferComplete(uint32_t callbackNumber) fisherton = 1; FillDshotBuffer(0x5555, GPIO_PIN_2); } - (void)(transferCompleteDetected); - if (HAL_DMA_Start_IT(&dmaHandles[ENUM_DMA2_STREAM_5], (uint32_t)&dshotOutBuffer, (uint32_t)&GPIOA->ODR, DSHOT_OUT_BUFFER_SIZE) != HAL_OK) - { - while(1); - } - //inlineDigitalHi(ports[ENUM_PORTA], GPIO_PIN_2); - TIM1->DIER |= 0x00000100; -} + */ -static void DshotDmaInit(void) //make general function? -{ - //init DMA - //for testing we hard code DMA2 Stream 5 - dmaHandles[ENUM_DMA2_STREAM_5].Instance = DMA2_Stream5; - dmaHandles[ENUM_DMA2_STREAM_5].Init.Channel = DMA_CHANNEL_6; - dmaHandles[ENUM_DMA2_STREAM_5].Init.Direction = DMA_MEMORY_TO_PERIPH; - dmaHandles[ENUM_DMA2_STREAM_5].Init.PeriphInc = DMA_PINC_DISABLE; - dmaHandles[ENUM_DMA2_STREAM_5].Init.MemInc = DMA_MINC_ENABLE; - dmaHandles[ENUM_DMA2_STREAM_5].Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; - dmaHandles[ENUM_DMA2_STREAM_5].Init.MemDataAlignment = DMA_MDATAALIGN_WORD; - dmaHandles[ENUM_DMA2_STREAM_5].Init.Mode = DMA_NORMAL; - dmaHandles[ENUM_DMA2_STREAM_5].Init.Priority = DMA_PRIORITY_HIGH; - dmaHandles[ENUM_DMA2_STREAM_5].Init.FIFOMode = DMA_FIFOMODE_DISABLE; - - HAL_DMA_UnRegisterCallback(&dmaHandles[ENUM_DMA2_STREAM_5], HAL_DMA_XFER_ALL_CB_ID); - - //only need the first one of these? - __HAL_LINKDMA(&dshotTimer,hdma[TIM_DMA_ID_UPDATE],dmaHandles[ENUM_DMA2_STREAM_5]); - __HAL_LINKDMA(&dshotTimer,hdma[TIM_DMA_ID_CC3],dmaHandles[ENUM_DMA2_STREAM_5]); - - - if (HAL_DMA_Init(&dmaHandles[ENUM_DMA2_STREAM_5]) != HAL_OK) + (void)(transferCompleteDetected); + if (HAL_DMA_Start_IT(&dmaHandles[ENUM_DMA2_STREAM_5], (uint32_t)&dshotOutBuffer, (uint32_t)&DSHOT_MOTOR_PORT->BSRR, DSHOT_OUT_BUFFER_SIZE) != HAL_OK) { while(1); } - HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 0, 2); - HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn); - - //register the callback function - callbackFunctionArray[FP_DMA2_S5] = DshotTransferComplete; + TIM1->DIER |= 0x00000100; } +/* -static void DshotTimerInit(uint32_t timer, uint32_t pwmHz, uint32_t timerHz) //make general function? -{ - - TIM_ClockConfigTypeDef sClockSourceConfig; //set, timer variable - TIM_MasterConfigTypeDef sMasterConfig; //set, timer variable - - (void)(pwmHz); - //run at 24 MHz, need a bitrate of 3 bits per byte: - //three types of "bytes" - //one, zero, nothing: - //one is XXx. - //zero is Xxx, - //nothing is xxx, - //Bit length (total timing period) is 1.67 microseconds - //For a bit to be 0, the pulse width is 625 nanoseconds - //For a bit to be 1, the pulse width is 1250 nanoseconds - //For DSHOT 600: - //15 and 24000000 MHz will generate a timer event ever 625 ns when the timer is active - //(15 / 24,000,000) = 0.000,000,625 seconds - //3 timer events per bit, 000 = nothing, 100 = zero, 110 = one - - dshotTimer.Instance = timers[timer]; - dshotTimer.Init.Prescaler = (uint16_t)(SystemCoreClock / TimerPrescalerDivisor(timer) / timerHz) - 1; - //dshotTimer.Init.CounterMode = TIM_COUNTERMODE_UP; - //dshotTimer.Init.Period = (timerHz / pwmHz) - 1;; - //dshotTimer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - //dshotTimer.Init.Prescaler = 1024; - dshotTimer.Init.CounterMode = TIM_COUNTERMODE_UP; - dshotTimer.Init.Period = 3; - dshotTimer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - if (HAL_TIM_Base_Init(&dshotTimer) != HAL_OK) - { - while(1); - } - sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(&dshotTimer, &sClockSourceConfig) != HAL_OK) - { - while(1); - } - sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&dshotTimer, &sMasterConfig) != HAL_OK) - { - while(1); - } -} void BlockingReadGPIO() {