void ledSet(led_t led, bool value) { if (led>LED_NUM) return; if (led_polarity[led]==LED_POL_NEG) value = !value; if(value==TRUE) GPIO_SetBits(led_port[led], led_pin[led]); else GPIO_ResetBits(led_port[led], led_pin[led]); #ifdef MOTORS_TEST if(led == LED_RED) { static int step = 0; if(!value) { motorsSetRatio(step, 0x3FFF); step++; if(step>3) step=0; } else { motorsSetRatio(0, 0x0000); motorsSetRatio(1, 0x0000); motorsSetRatio(2, 0x0000); motorsSetRatio(3, 0x0000); } } #endif }
void powerDistribution(const control_t *control) { #ifdef QUAD_FORMATION_X int16_t r = control->roll / 2.0f; int16_t p = control->pitch / 2.0f; motorPower.m1 = limitThrust(control->thrust - r + p + control->yaw); motorPower.m2 = limitThrust(control->thrust - r - p - control->yaw); motorPower.m3 = limitThrust(control->thrust + r - p + control->yaw); motorPower.m4 = limitThrust(control->thrust + r + p - control->yaw); #else // QUAD_FORMATION_NORMAL motorPower.m1 = limitThrust(control->thrust + control->pitch + control->yaw); motorPower.m2 = limitThrust(control->thrust - control->roll - control->yaw); motorPower.m3 = limitThrust(control->thrust - control->pitch + control->yaw); motorPower.m4 = limitThrust(control->thrust + control->roll - control->yaw); #endif if (motorSetEnable) { motorsSetRatio(MOTOR_M1, motorPowerSet.m1); motorsSetRatio(MOTOR_M2, motorPowerSet.m2); motorsSetRatio(MOTOR_M3, motorPowerSet.m3); motorsSetRatio(MOTOR_M4, motorPowerSet.m4); } else { motorsSetRatio(MOTOR_M1, motorPower.m1); motorsSetRatio(MOTOR_M2, motorPower.m2); motorsSetRatio(MOTOR_M3, motorPower.m3); motorsSetRatio(MOTOR_M4, motorPower.m4); } }
// FreeRTOS Task to test the Motors driver with a rampup of each motor alone. void motorsTestTask(void* params) { int step=0; float rampup = 0.01; motorsSetupMinMaxPos(); motorsSetRatio(MOTOR_LEFT, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_REAR, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_RIGHT, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_FRONT, 1*(1<<16) * 0.0); vTaskDelay(M2T(1000)); while(1) { vTaskDelay(M2T(100)); motorsSetRatio(MOTOR_LEFT, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_REAR, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_RIGHT, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_FRONT, 1*(1<<16) * rampup); rampup += 0.001; if (rampup >= 0.1) { if(++step>3) step=0; rampup = 0.01; } } }
// FreeRTOS Task to test the Motors driver with a rampup of each motor alone. void motorsTestTask(void* params) { int step=0; float rampup = 0.01; motorsSetRatio(MOTOR_M4, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_M3, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_M2, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_M1, 1*(1<<16) * 0.0); vTaskDelay(M2T(1000)); while(1) { vTaskDelay(M2T(100)); motorsSetRatio(MOTOR_M4, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_M3, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_M2, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_M1, 1*(1<<16) * rampup); rampup += 0.001; if (rampup >= 0.1) { if(++step>3) step=0; rampup = 0.01; } } }
rt_bool_t motorsTest(void) { #ifndef BRUSHLESS_MOTORCONTROLLER int i; for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++) { motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO); rt_thread_delay(M2T(MOTORS_TEST_ON_TIME_MS)); motorsSetRatio(MOTORS[i], 0); rt_thread_delay(M2T(MOTORS_TEST_DELAY_TIME_MS)); } #endif return isInit; }
void assertFail(char *exp, char *file, int line) { portDISABLE_INTERRUPTS(); storeAssertSnapshotData(file, line); DEBUG_PRINT("Assert failed %s:%d\n", file, line); motorsSetRatio(MOTOR_M1, 0); motorsSetRatio(MOTOR_M2, 0); motorsSetRatio(MOTOR_M3, 0); motorsSetRatio(MOTOR_M4, 0); ledClearAll(); ledSet(ERR_LED1, 1); ledSet(ERR_LED2, 1); while (1); }
// FreeRTOS Task to test the Motors driver void motorsTestTask(void* params) { static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)}; int step=0; //Wait 3 seconds before starting the motors vTaskDelay(M2T(3000)); while(1) { motorsSetRatio(MOTOR_LEFT, sequence[step%4]); motorsSetRatio(MOTOR_REAR, sequence[(step+1)%4]); motorsSetRatio(MOTOR_RIGHT, sequence[(step+2)%4]); motorsSetRatio(MOTOR_FRONT, sequence[(step+3)%4]); if(++step>3) step=0; vTaskDelay(M2T(1000)); } }
// FreeRTOS Task to test the Motors driver void motorsTestTask(void* params) { static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)}; int step=0; //Wait 3 seconds before starting the motors rt_thread_delay(M2T(3000)); while(1) { motorsSetRatio(MOTOR_M4, sequence[step%4]); motorsSetRatio(MOTOR_M3, sequence[(step+1)%4]); motorsSetRatio(MOTOR_M2, sequence[(step+2)%4]); motorsSetRatio(MOTOR_M1, sequence[(step+3)%4]); if(++step>3) step=0; rt_thread_delay(M2T(1000)); } }
bool motorsTest(void) { motorsSetRatio(MOTOR_FRONT, MOTORS_TEST_RATIO); vTaskDelay(MOTORS_TEST_ON_TIME); motorsSetRatio(MOTOR_FRONT, 0); vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME)); motorsSetRatio(MOTOR_RIGHT, MOTORS_TEST_RATIO); vTaskDelay(M2T(MOTORS_TEST_ON_TIME)); motorsSetRatio(MOTOR_RIGHT, 0); vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME)); motorsSetRatio(MOTOR_REAR, MOTORS_TEST_RATIO); vTaskDelay(M2T(MOTORS_TEST_ON_TIME)); motorsSetRatio(MOTOR_REAR, 0); vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME)); motorsSetRatio(MOTOR_LEFT, MOTORS_TEST_RATIO); vTaskDelay(M2T(MOTORS_TEST_ON_TIME)); motorsSetRatio(MOTOR_LEFT, 0); return isInit; }
void motorsInit() { #ifdef M451 CLK_EnableModuleClock(PWM0_MODULE); CLK_SetModuleClock(PWM0_MODULE, CLK_CLKSEL2_PWM0SEL_PCLK0, 0); SYS->GPC_MFPL = (SYS->GPC_MFPL & (~SYS_GPC_MFPL_PC0MFP_Msk) & (~SYS_GPC_MFPL_PC1MFP_Msk) & (~SYS_GPC_MFPL_PC2MFP_Msk) & (~SYS_GPC_MFPL_PC3MFP_Msk) #ifdef HEX6X & (~SYS_GPC_MFPL_PC4MFP_Msk) #endif ); #ifdef HEX6X SYS->GPD_MFPL = (SYS->GPD_MFPL & (~SYS_GPD_MFPL_PD7MFP_Msk) ); #endif SYS->GPC_MFPL |= (SYS_GPC_MFPL_PC0MFP_PWM0_CH0|SYS_GPC_MFPL_PC1MFP_PWM0_CH1| SYS_GPC_MFPL_PC2MFP_PWM0_CH2|SYS_GPC_MFPL_PC3MFP_PWM0_CH3 #ifdef HEX6X |SYS_GPC_MFPL_PC4MFP_PWM0_CH4 #endif ); #ifdef HEX6X SYS->GPD_MFPL |= (SYS_GPD_MFPL_PD7MFP_PWM0_CH5); #endif PWM_ConfigOutputChannelf(PWM0, 0, ESC_UPDATE_FREQ, ESC_UPDATE_FREQ/10); PWM_ConfigOutputChannelf(PWM0, 1, ESC_UPDATE_FREQ, ESC_UPDATE_FREQ/10); PWM_ConfigOutputChannelf(PWM0, 2, ESC_UPDATE_FREQ, ESC_UPDATE_FREQ/10); PWM_ConfigOutputChannelf(PWM0, 3, ESC_UPDATE_FREQ, ESC_UPDATE_FREQ/10); #ifdef HEX6X PWM_ConfigOutputChannelf(PWM0, 4, ESC_UPDATE_FREQ, ESC_UPDATE_FREQ/10); PWM_ConfigOutputChannelf(PWM0, 5, ESC_UPDATE_FREQ, ESC_UPDATE_FREQ/10); #endif motorsSetRatio(MOTOR_M1, 0); motorsSetRatio(MOTOR_M2, 0); motorsSetRatio(MOTOR_M3, 0); motorsSetRatio(MOTOR_M4, 0); #ifdef HEX6X motorsSetRatio(MOTOR_M5, 0); motorsSetRatio(MOTOR_M6, 0); #endif PWM_EnableOutput(PWM0, PWM_CH_0_MASK); PWM_EnableOutput(PWM0, PWM_CH_1_MASK); PWM_EnableOutput(PWM0, PWM_CH_2_MASK); PWM_EnableOutput(PWM0, PWM_CH_3_MASK); #ifdef HEX6X PWM_EnableOutput(PWM0, PWM_CH_4_MASK); PWM_EnableOutput(PWM0, PWM_CH_5_MASK); #endif #else uint8_t u8Timer; DrvPWM_Open(); DrvGPIO_InitFunction(E_FUNC_PWM01); DrvGPIO_InitFunction(E_FUNC_PWM23); s_u32Pulse = 0; g_u8PWMCount = 1; g_u16Frequency = ESC_UPDATE_FREQ; sPt.u8Mode = DRVPWM_AUTO_RELOAD_MODE; sPt.u32Frequency = g_u16Frequency; sPt.u8HighPulseRatio = 1; sPt.u8HighPulseBase = 1000000/g_u16Frequency; sPt.i32Inverter = 0; u8Timer = DRVPWM_TIMER0; DrvPWM_SelectClockSource(u8Timer, DRVPWM_HCLK); DrvPWM_SetTimerClk(u8Timer, &sPt); DrvPWM_SetTimerIO(u8Timer, 1); u8Timer = DRVPWM_TIMER1; DrvPWM_SelectClockSource(u8Timer, DRVPWM_HCLK); DrvPWM_SetTimerClk(u8Timer, &sPt); DrvPWM_SetTimerIO(u8Timer, 1); u8Timer = DRVPWM_TIMER2; DrvPWM_SelectClockSource(u8Timer, DRVPWM_HCLK); DrvPWM_SetTimerClk(u8Timer, &sPt); DrvPWM_SetTimerIO(u8Timer, 1); u8Timer = DRVPWM_TIMER3; DrvPWM_SelectClockSource(u8Timer, DRVPWM_HCLK); DrvPWM_SetTimerClk(u8Timer, &sPt); DrvPWM_SetTimerIO(u8Timer, 1); #endif }
static void distributePower(const uint16_t thrust, const int16_t roll, const int16_t pitch, const int16_t yaw) { float motors_max; float motors_min; float motors_spread; motor_saturated = false; int16_t r = roll >> 1; int16_t p = pitch >> 1; motorPowerM1 = thrust - r + p + yaw; motorPowerM2 = thrust - r - p - yaw; motorPowerM3 = thrust + r - p + yaw; motorPowerM4 = thrust + r + p - yaw; motors_max = max(motorPowerM1,max(motorPowerM2,max(motorPowerM3,motorPowerM4))); motors_min = min(motorPowerM1,min(motorPowerM2,min(motorPowerM3,motorPowerM4))); motors_spread = motors_max - motors_min; //"Air Mode" - Maintain the total commanded differences in thrust to provide better control at low and high throttle. if (motors_spread > UINT16_MAX) { motor_saturated = true; motorPowerM1 -= motors_min; motorPowerM2 -= motors_min; motorPowerM3 -= motors_min; motorPowerM4 -= motors_min; motors_max -= motors_min; motors_spread = (motors_max-motors_min)/UINT16_MAX; motorPowerM1 /= motors_spread; motorPowerM2 /= motors_spread; motorPowerM3 /= motors_spread; motorPowerM4 /= motors_spread; } else if (motors_min < 0) { motor_saturated = true; motorPowerM1 -= motors_min; motorPowerM2 -= motors_min; motorPowerM3 -= motors_min; motorPowerM4 -= motors_min; } else if (motors_max > UINT16_MAX) { motor_saturated = true; motors_max -= UINT16_MAX; motorPowerM1 -= motors_max; motorPowerM2 -= motors_max; motorPowerM3 -= motors_max; motorPowerM4 -= motors_max; } //In case of floating point rounding weirdness motorPowerM1 = limitThrust(motorPowerM1); motorPowerM2 = limitThrust(motorPowerM2); motorPowerM3 = limitThrust(motorPowerM3); motorPowerM4 = limitThrust(motorPowerM4); motorsSetRatio(MOTOR_M1, motorPowerM1); motorsSetRatio(MOTOR_M2, motorPowerM2); motorsSetRatio(MOTOR_M3, motorPowerM3); motorsSetRatio(MOTOR_M4, motorPowerM4); }