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);
}
