Наведён порядок в коде
This commit is contained in:
Radzhab Bisultanov
@@ -1,119 +1,142 @@
|
||||
#include "imu.h"
|
||||
|
||||
void imu_pow_init()
|
||||
{
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
|
||||
GPIOC->MODER &= ~(3 << (13 * 2));
|
||||
GPIOC->MODER |= 1 << (13 * 2);
|
||||
GPIOC->OTYPER &= ~(1 << 13);
|
||||
GPIOC->PUPDR &= ~(3 << (13 * 2));
|
||||
GPIOC->BSRR = 1 << (13 + 16);
|
||||
}
|
||||
|
||||
void i2c_gpio_init()
|
||||
{
|
||||
// enable GPIOB clock
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
|
||||
|
||||
// Alt function mode PB8, PB9
|
||||
GPIOB->MODER &= ~((3 << 8 * 2) | (3 << 9 * 2));
|
||||
GPIOB->MODER |= (2 << (8 * 2)) | (2 << (9 * 2));
|
||||
|
||||
// select AF4 for I2C1 at PB8 and PB9
|
||||
GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
|
||||
GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
|
||||
|
||||
// enable open-drain
|
||||
GPIOB->OTYPER |= (1 << 8) | (1 << 9);
|
||||
|
||||
// set pull-up
|
||||
GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
|
||||
GPIOB->PUPDR |= (1 << 8 * 2) | (1 << 9 * 2);
|
||||
// enable GPIOB clock
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
|
||||
|
||||
// Alt function mode PB8, PB9
|
||||
GPIOB->MODER &= ~((3 << 8 * 2) | (3 << 9 * 2));
|
||||
GPIOB->MODER |= (2 << (8 * 2)) | (2 << (9 * 2));
|
||||
|
||||
// select AF4 for I2C1 at PB8 and PB9
|
||||
GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
|
||||
GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
|
||||
|
||||
// enable open-drain
|
||||
GPIOB->OTYPER |= (1 << 8) | (1 << 9);
|
||||
|
||||
// set pull-up
|
||||
GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
|
||||
GPIOB->PUPDR |= (1 << 8 * 2) | (1 << 9 * 2);
|
||||
}
|
||||
|
||||
void i2c1_init()
|
||||
{
|
||||
// enable I2C1
|
||||
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
|
||||
|
||||
// 400 kHz @ 16 MHz
|
||||
I2C1->TIMINGR = 0x00303D5B;
|
||||
|
||||
I2C1->CR1 |= I2C_CR1_PE;
|
||||
// enable I2C1
|
||||
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
|
||||
|
||||
// 400 kHz @ 16 MHz
|
||||
I2C1->TIMINGR = 0x00303D5B;
|
||||
|
||||
I2C1->CR1 |= I2C_CR1_PE;
|
||||
}
|
||||
|
||||
void icm_init()
|
||||
void imu_init()
|
||||
{
|
||||
// select bank 0
|
||||
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
|
||||
|
||||
// wake up, auto clock
|
||||
i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
|
||||
|
||||
// select bank 2
|
||||
i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
|
||||
|
||||
// gyro ~2000 dps, FS_SEL = 3
|
||||
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (3 << 1));
|
||||
|
||||
// accel ~4g, FS_SEL = 1
|
||||
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
|
||||
|
||||
// back to bank 0
|
||||
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
|
||||
// select bank 0
|
||||
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
|
||||
|
||||
// wake up, auto clock
|
||||
i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
|
||||
|
||||
// select bank 2
|
||||
i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
|
||||
|
||||
// gyro ~2000 dps, FS_SEL = 3
|
||||
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (3 << 1));
|
||||
|
||||
// accel ~4g, FS_SEL = 1
|
||||
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
|
||||
|
||||
// back to bank 0
|
||||
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
|
||||
}
|
||||
|
||||
void imu_tim6_init()
|
||||
{
|
||||
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
|
||||
|
||||
TIM6->PSC = 16000 - 1; // 16 MHz / 16000 = 1000 Hz (1 ms)
|
||||
TIM6->ARR = 2 - 1; // 2 ms
|
||||
|
||||
TIM6->DIER |= TIM_DIER_UIE; // interrupt enable
|
||||
TIM6->CR1 |= TIM_CR1_CEN; // counter enable
|
||||
|
||||
NVIC_EnableIRQ(TIM6_DAC_IRQn);
|
||||
}
|
||||
|
||||
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
|
||||
{
|
||||
i2c_wait_idle(I2C1);
|
||||
|
||||
// write register address
|
||||
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TXIS));
|
||||
I2C1->TXDR = reg;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TC));
|
||||
|
||||
I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
|
||||
|
||||
for (uint8_t i = 0; i < len; ++i)
|
||||
{
|
||||
while (!(I2C1->ISR & I2C_ISR_RXNE));
|
||||
buf[i] = I2C1->RXDR;
|
||||
}
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_STOPF));
|
||||
I2C1->ICR |= I2C_ICR_STOPCF;
|
||||
i2c_wait_idle(I2C1);
|
||||
|
||||
// write register address
|
||||
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TXIS));
|
||||
I2C1->TXDR = reg;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TC));
|
||||
|
||||
I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
|
||||
|
||||
for (uint8_t i = 0; i < len; ++i)
|
||||
{
|
||||
while (!(I2C1->ISR & I2C_ISR_RXNE));
|
||||
buf[i] = I2C1->RXDR;
|
||||
}
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_STOPF));
|
||||
I2C1->ICR |= I2C_ICR_STOPCF;
|
||||
}
|
||||
|
||||
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
|
||||
{
|
||||
i2c_wait_idle(I2C1);
|
||||
|
||||
// write register address
|
||||
I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TXIS));
|
||||
I2C1->TXDR = reg;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TXIS));
|
||||
I2C1->TXDR = data;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TC));
|
||||
I2C1->CR2 |= I2C_CR2_STOP;
|
||||
i2c_wait_idle(I2C1);
|
||||
|
||||
// write register address
|
||||
I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TXIS));
|
||||
I2C1->TXDR = reg;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TXIS));
|
||||
I2C1->TXDR = data;
|
||||
|
||||
while (!(I2C1->ISR & I2C_ISR_TC));
|
||||
I2C1->CR2 |= I2C_CR2_STOP;
|
||||
}
|
||||
|
||||
void icm_read_raw(imu_raw_t* data)
|
||||
void imu_read_raw(imu_raw_t* data)
|
||||
{
|
||||
uint8_t buf[12];
|
||||
|
||||
i2c_read(ICM_ADDR, 0x2D, buf, 12);
|
||||
|
||||
data->ax = (buf[0] << 8) | buf[1];
|
||||
data->ay = (buf[2] << 8) | buf[3];
|
||||
data->az = (buf[4] << 8) | buf[5];
|
||||
|
||||
data->gx = (buf[6] << 8) | buf[7];
|
||||
data->gy = (buf[8] << 8) | buf[9];
|
||||
data->gz = (buf[10] << 8) | buf[11];
|
||||
uint8_t buf[12];
|
||||
|
||||
i2c_read(ICM_ADDR, 0x2D, buf, 12);
|
||||
|
||||
data->ax = (buf[0] << 8) | buf[1];
|
||||
data->ay = (buf[2] << 8) | buf[3];
|
||||
data->az = (buf[4] << 8) | buf[5];
|
||||
|
||||
data->gx = (buf[6] << 8) | buf[7];
|
||||
data->gy = (buf[8] << 8) | buf[9];
|
||||
data->gz = (buf[10] << 8) | buf[11];
|
||||
}
|
||||
|
||||
static void i2c_wait_idle(I2C_TypeDef* i2c)
|
||||
{
|
||||
while (i2c->ISR & I2C_ISR_BUSY);
|
||||
|
||||
i2c->ICR = I2C_ICR_STOPCF |
|
||||
while (i2c->ISR & I2C_ISR_BUSY);
|
||||
|
||||
i2c->ICR = I2C_ICR_STOPCF |
|
||||
I2C_ICR_NACKCF |
|
||||
I2C_ICR_BERRCF |
|
||||
I2C_ICR_ARLOCF;
|
||||
|
||||
@@ -20,72 +20,72 @@ static biquad_t gyro_z_lpf;
|
||||
|
||||
void imu_processing_init()
|
||||
{
|
||||
biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
|
||||
biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
|
||||
biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
|
||||
|
||||
biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
|
||||
biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
|
||||
biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
|
||||
biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
|
||||
biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
|
||||
biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
|
||||
|
||||
biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
|
||||
biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
|
||||
biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
|
||||
}
|
||||
|
||||
void imu_read_scaled(imu_scaled_t* out)
|
||||
{
|
||||
static imu_raw_t raw;
|
||||
|
||||
icm_read_raw(&raw);
|
||||
|
||||
out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
|
||||
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
|
||||
out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
|
||||
|
||||
out->ax = biquad_apply(&accel_x_lpf, out->ax);
|
||||
out->ay = biquad_apply(&accel_y_lpf, out->ay);
|
||||
out->az = biquad_apply(&accel_z_lpf, out->az);
|
||||
|
||||
out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
|
||||
out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
|
||||
out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
|
||||
|
||||
out->gx = biquad_apply(&gyro_x_lpf, out->gx);
|
||||
out->gy = biquad_apply(&gyro_y_lpf, out->gy);
|
||||
out->gz = biquad_apply(&gyro_z_lpf, out->gz);
|
||||
static imu_raw_t raw;
|
||||
|
||||
imu_read_raw(&raw);
|
||||
|
||||
out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
|
||||
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
|
||||
out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
|
||||
|
||||
out->ax = biquad_apply(&accel_x_lpf, out->ax);
|
||||
out->ay = biquad_apply(&accel_y_lpf, out->ay);
|
||||
out->az = biquad_apply(&accel_z_lpf, out->az);
|
||||
|
||||
out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
|
||||
out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
|
||||
out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
|
||||
|
||||
out->gx = biquad_apply(&gyro_x_lpf, out->gx);
|
||||
out->gy = biquad_apply(&gyro_y_lpf, out->gy);
|
||||
out->gz = biquad_apply(&gyro_z_lpf, out->gz);
|
||||
}
|
||||
|
||||
void imu_calibrate()
|
||||
{
|
||||
const int samples = 1000;
|
||||
|
||||
float sum_ax = 0;
|
||||
float sum_ay = 0;
|
||||
float sum_az = 0;
|
||||
|
||||
float sum_gx = 0;
|
||||
float sum_gy = 0;
|
||||
float sum_gz = 0;
|
||||
|
||||
imu_raw_t imu;
|
||||
|
||||
for (uint16_t i = 0; i < samples; ++i)
|
||||
{
|
||||
icm_read_raw(&imu);
|
||||
|
||||
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
|
||||
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
|
||||
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
|
||||
|
||||
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
|
||||
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
|
||||
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
|
||||
|
||||
for (volatile uint16_t d = 0; d < 5000; ++d);
|
||||
}
|
||||
|
||||
accel_bias_x = sum_ax / samples;
|
||||
accel_bias_y = sum_ay / samples;
|
||||
accel_bias_z = (sum_az / samples) + 1.0f;
|
||||
|
||||
gyro_bias_x = sum_gx / samples;
|
||||
gyro_bias_y = sum_gy / samples;
|
||||
gyro_bias_z = sum_gz / samples;
|
||||
const int samples = 1000;
|
||||
|
||||
float sum_ax = 0;
|
||||
float sum_ay = 0;
|
||||
float sum_az = 0;
|
||||
|
||||
float sum_gx = 0;
|
||||
float sum_gy = 0;
|
||||
float sum_gz = 0;
|
||||
|
||||
imu_raw_t imu;
|
||||
|
||||
for (uint16_t i = 0; i < samples; ++i)
|
||||
{
|
||||
imu_read_raw(&imu);
|
||||
|
||||
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
|
||||
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
|
||||
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
|
||||
|
||||
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
|
||||
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
|
||||
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
|
||||
|
||||
for (volatile uint16_t d = 0; d < 5000; ++d);
|
||||
}
|
||||
|
||||
accel_bias_x = sum_ax / samples;
|
||||
accel_bias_y = sum_ay / samples;
|
||||
accel_bias_z = (sum_az / samples) + 1.0f;
|
||||
|
||||
gyro_bias_x = sum_gx / samples;
|
||||
gyro_bias_y = sum_gy / samples;
|
||||
gyro_bias_z = sum_gz / samples;
|
||||
}
|
||||
|
||||
@@ -3,143 +3,143 @@
|
||||
|
||||
void motors_init()
|
||||
{
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
|
||||
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
|
||||
|
||||
motor_gpio_tim1_ch3_init();
|
||||
motor_gpio_tim1_ch4_init();
|
||||
motors_tim1_ch3_4_init();
|
||||
|
||||
motor_gpio_tim2_ch1_init();
|
||||
motor_gpio_tim2_ch2_init();
|
||||
motors_tim2_ch1_2_init();
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
|
||||
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
|
||||
|
||||
motor_gpio_tim1_ch3_init();
|
||||
motor_gpio_tim1_ch4_init();
|
||||
motors_tim1_ch3_4_init();
|
||||
|
||||
motor_gpio_tim2_ch1_init();
|
||||
motor_gpio_tim2_ch2_init();
|
||||
motors_tim2_ch1_2_init();
|
||||
}
|
||||
|
||||
void motor_gpio_tim1_ch3_init()
|
||||
{
|
||||
// set alt function mode PA10
|
||||
GPIOA->MODER &= ~(3 << (10 * 2));
|
||||
GPIOA->MODER |= 2 << (10 * 2);
|
||||
|
||||
// AF6 for PA10
|
||||
GPIOA->AFR[1] &= ~(0xF << 8);
|
||||
GPIOA->AFR[1] |= 6 << 8;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (10 * 2);
|
||||
// set alt function mode PA10
|
||||
GPIOA->MODER &= ~(3 << (10 * 2));
|
||||
GPIOA->MODER |= 2 << (10 * 2);
|
||||
|
||||
// AF6 for PA10
|
||||
GPIOA->AFR[1] &= ~(0xF << 8);
|
||||
GPIOA->AFR[1] |= 6 << 8;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (10 * 2);
|
||||
}
|
||||
|
||||
void motor_gpio_tim1_ch4_init()
|
||||
{
|
||||
// set alt function mode PA11
|
||||
GPIOA->MODER &= ~(3 << (11 * 2));
|
||||
GPIOA->MODER |= 2 << (11 * 2);
|
||||
|
||||
// AF11 for PA11
|
||||
GPIOA->AFR[1] &= ~(0xF << 12);
|
||||
GPIOA->AFR[1] |= 11 << 12;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (11 * 2);
|
||||
// set alt function mode PA11
|
||||
GPIOA->MODER &= ~(3 << (11 * 2));
|
||||
GPIOA->MODER |= 2 << (11 * 2);
|
||||
|
||||
// AF11 for PA11
|
||||
GPIOA->AFR[1] &= ~(0xF << 12);
|
||||
GPIOA->AFR[1] |= 11 << 12;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (11 * 2);
|
||||
}
|
||||
|
||||
void motors_tim1_ch3_4_init()
|
||||
{
|
||||
// PWM mode 1
|
||||
TIM1->CCMR2 &= ~TIM_CCMR2_OC3M;
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2;
|
||||
|
||||
TIM1->CCMR2 &= ~TIM_CCMR2_OC4M;
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2;
|
||||
|
||||
// preload enable
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC3PE;
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC4PE;
|
||||
|
||||
// enable capture/compare 3 output
|
||||
TIM1->CCER |= TIM_CCER_CC3E;
|
||||
TIM1->CCER |= TIM_CCER_CC4E;
|
||||
|
||||
TIM1->PSC = 16 - 1;
|
||||
TIM1->ARR = 20000 - 1;
|
||||
TIM1->CCR3 = 900;
|
||||
TIM1->CCR4 = 900;
|
||||
|
||||
// TIM1_ARR is buffered
|
||||
TIM1->CR1 |= TIM_CR1_ARPE;
|
||||
|
||||
// set main output enable
|
||||
TIM1->BDTR |= TIM_BDTR_MOE;
|
||||
|
||||
// set update generation
|
||||
TIM1->EGR |= TIM_EGR_UG;
|
||||
|
||||
// set counter enable
|
||||
TIM1->CR1 |= TIM_CR1_CEN;
|
||||
// PWM mode 1
|
||||
TIM1->CCMR2 &= ~TIM_CCMR2_OC3M;
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2;
|
||||
|
||||
TIM1->CCMR2 &= ~TIM_CCMR2_OC4M;
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2;
|
||||
|
||||
// preload enable
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC3PE;
|
||||
TIM1->CCMR2 |= TIM_CCMR2_OC4PE;
|
||||
|
||||
// enable capture/compare 3 output
|
||||
TIM1->CCER |= TIM_CCER_CC3E;
|
||||
TIM1->CCER |= TIM_CCER_CC4E;
|
||||
|
||||
TIM1->PSC = 16 - 1;
|
||||
TIM1->ARR = 20000 - 1;
|
||||
TIM1->CCR3 = 900;
|
||||
TIM1->CCR4 = 900;
|
||||
|
||||
// TIM1_ARR is buffered
|
||||
TIM1->CR1 |= TIM_CR1_ARPE;
|
||||
|
||||
// set main output enable
|
||||
TIM1->BDTR |= TIM_BDTR_MOE;
|
||||
|
||||
// set update generation
|
||||
TIM1->EGR |= TIM_EGR_UG;
|
||||
|
||||
// set counter enable
|
||||
TIM1->CR1 |= TIM_CR1_CEN;
|
||||
}
|
||||
|
||||
void motor_gpio_tim2_ch1_init()
|
||||
{
|
||||
// set alt function mode PA0
|
||||
GPIOA->MODER &= ~(3 << (0 * 2));
|
||||
GPIOA->MODER |= 2 << (0 * 2);
|
||||
|
||||
// AF1 for PA0
|
||||
GPIOA->AFR[0] &= ~(0xF << 0);
|
||||
GPIOA->AFR[0] |= 1 << 0;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (0 * 2);
|
||||
// set alt function mode PA0
|
||||
GPIOA->MODER &= ~(3 << (0 * 2));
|
||||
GPIOA->MODER |= 2 << (0 * 2);
|
||||
|
||||
// AF1 for PA0
|
||||
GPIOA->AFR[0] &= ~(0xF << 0);
|
||||
GPIOA->AFR[0] |= 1 << 0;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (0 * 2);
|
||||
}
|
||||
|
||||
void motor_gpio_tim2_ch2_init()
|
||||
{
|
||||
// set alt function mode PA1
|
||||
GPIOA->MODER &= ~(3 << (1 * 2));
|
||||
GPIOA->MODER |= 2 << (1 * 2);
|
||||
|
||||
// AF1 for PA1
|
||||
GPIOA->AFR[0] &= ~(0xF << 4);
|
||||
GPIOA->AFR[0] |= 1 << 4;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (1 * 2);
|
||||
// set alt function mode PA1
|
||||
GPIOA->MODER &= ~(3 << (1 * 2));
|
||||
GPIOA->MODER |= 2 << (1 * 2);
|
||||
|
||||
// AF1 for PA1
|
||||
GPIOA->AFR[0] &= ~(0xF << 4);
|
||||
GPIOA->AFR[0] |= 1 << 4;
|
||||
|
||||
// very high speed
|
||||
GPIOA->OSPEEDR |= 3 << (1 * 2);
|
||||
}
|
||||
|
||||
void motors_tim2_ch1_2_init()
|
||||
{
|
||||
// PWM mode 1
|
||||
TIM2->CCMR1 &= ~TIM_CCMR1_OC1M;
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
|
||||
|
||||
TIM2->CCMR1 &= ~TIM_CCMR1_OC2M;
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2;
|
||||
|
||||
// preload enable
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC1PE;
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC2PE;
|
||||
|
||||
// enable capture/compare 3 output
|
||||
TIM2->CCER |= TIM_CCER_CC1E;
|
||||
TIM2->CCER |= TIM_CCER_CC2E;
|
||||
|
||||
TIM2->PSC = 16 - 1;
|
||||
TIM2->ARR = 20000 - 1;
|
||||
TIM2->CCR1 = 900;
|
||||
TIM2->CCR2 = 900;
|
||||
|
||||
// TIM2_ARR is buffered
|
||||
TIM2->CR1 |= TIM_CR1_ARPE;
|
||||
|
||||
// set main output enable
|
||||
TIM2->BDTR |= TIM_BDTR_MOE;
|
||||
|
||||
// set update generation
|
||||
TIM2->EGR |= TIM_EGR_UG;
|
||||
|
||||
// set counter enable
|
||||
TIM2->CR1 |= TIM_CR1_CEN;
|
||||
// PWM mode 1
|
||||
TIM2->CCMR1 &= ~TIM_CCMR1_OC1M;
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
|
||||
|
||||
TIM2->CCMR1 &= ~TIM_CCMR1_OC2M;
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2;
|
||||
|
||||
// preload enable
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC1PE;
|
||||
TIM2->CCMR1 |= TIM_CCMR1_OC2PE;
|
||||
|
||||
// enable capture/compare 3 output
|
||||
TIM2->CCER |= TIM_CCER_CC1E;
|
||||
TIM2->CCER |= TIM_CCER_CC2E;
|
||||
|
||||
TIM2->PSC = 16 - 1;
|
||||
TIM2->ARR = 20000 - 1;
|
||||
TIM2->CCR1 = 900;
|
||||
TIM2->CCR2 = 900;
|
||||
|
||||
// TIM2_ARR is buffered
|
||||
TIM2->CR1 |= TIM_CR1_ARPE;
|
||||
|
||||
// set main output enable
|
||||
TIM2->BDTR |= TIM_BDTR_MOE;
|
||||
|
||||
// set update generation
|
||||
TIM2->EGR |= TIM_EGR_UG;
|
||||
|
||||
// set counter enable
|
||||
TIM2->CR1 |= TIM_CR1_CEN;
|
||||
}
|
||||
|
||||
int16_t T;
|
||||
@@ -154,77 +154,77 @@ int16_t m4;
|
||||
|
||||
void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed)
|
||||
{
|
||||
T = throttle;
|
||||
P = (int16_t) chs->pitch;
|
||||
R = (int16_t) chs->roll;
|
||||
Y = (int16_t) chs->yaw;
|
||||
|
||||
m1 = T - P + R - Y;
|
||||
m2 = T - P - R + Y;
|
||||
m3 = T + P + R + Y;
|
||||
m4 = T + P - R - Y;
|
||||
|
||||
motor_set_throttle(1, m1, armed);
|
||||
motor_set_throttle(2, m2, armed);
|
||||
motor_set_throttle(3, m3, armed);
|
||||
motor_set_throttle(4, m4, armed);
|
||||
T = throttle;
|
||||
P = (int16_t) chs->pitch;
|
||||
R = (int16_t) chs->roll;
|
||||
Y = (int16_t) chs->yaw;
|
||||
|
||||
m1 = T - P + R - Y;
|
||||
m2 = T - P - R + Y;
|
||||
m3 = T + P + R + Y;
|
||||
m4 = T + P - R - Y;
|
||||
|
||||
motor_set_throttle(1, m1, armed);
|
||||
motor_set_throttle(2, m2, armed);
|
||||
motor_set_throttle(3, m3, armed);
|
||||
motor_set_throttle(4, m4, armed);
|
||||
}
|
||||
|
||||
void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed)
|
||||
{
|
||||
if (armed && us < 1050) us = 1050;
|
||||
if (us > 2000) us = 2000;
|
||||
|
||||
switch (motor_number)
|
||||
{
|
||||
case 1:
|
||||
TIM1->CCR3 = us;
|
||||
break;
|
||||
case 2:
|
||||
TIM2->CCR2 = us;
|
||||
break;
|
||||
case 3:
|
||||
TIM1->CCR4 = us;
|
||||
break;
|
||||
case 4:
|
||||
TIM2->CCR1 = us;
|
||||
break;
|
||||
}
|
||||
if (armed && us < 1050) us = 1050;
|
||||
if (us > 2000) us = 2000;
|
||||
|
||||
switch (motor_number)
|
||||
{
|
||||
case 1:
|
||||
TIM1->CCR3 = us;
|
||||
break;
|
||||
case 2:
|
||||
TIM2->CCR2 = us;
|
||||
break;
|
||||
case 3:
|
||||
TIM1->CCR4 = us;
|
||||
break;
|
||||
case 4:
|
||||
TIM2->CCR1 = us;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void motors_turn_off()
|
||||
{
|
||||
motor_set_throttle(1, 900, 0);
|
||||
motor_set_throttle(2, 900, 0);
|
||||
motor_set_throttle(3, 900, 0);
|
||||
motor_set_throttle(4, 900, 0);
|
||||
motor_set_throttle(1, 900, 0);
|
||||
motor_set_throttle(2, 900, 0);
|
||||
motor_set_throttle(3, 900, 0);
|
||||
motor_set_throttle(4, 900, 0);
|
||||
}
|
||||
|
||||
void motor1_set_throttle(int16_t throttle)
|
||||
{
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM1->CCR3 = throttle;
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM1->CCR3 = throttle;
|
||||
}
|
||||
|
||||
void motor2_set_throttle(int16_t throttle)
|
||||
{
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM1->CCR4 = throttle;
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM1->CCR4 = throttle;
|
||||
}
|
||||
|
||||
void motor3_set_throttle(int16_t throttle)
|
||||
{
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM2->CCR1 = throttle;
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM2->CCR1 = throttle;
|
||||
}
|
||||
|
||||
void motor4_set_throttle(int16_t throttle)
|
||||
{
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM2->CCR2 = throttle;
|
||||
if (throttle < 1000) throttle = 1000;
|
||||
if (throttle > 2000) throttle = 2000;
|
||||
TIM2->CCR2 = throttle;
|
||||
}
|
||||
|
||||
|
||||
@@ -10,196 +10,159 @@ volatile uint8_t sbus_frame_ready = 0;
|
||||
|
||||
void receiver_gpio_init()
|
||||
{
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||
|
||||
GPIOA->MODER &= ~(3 << (3 * 2));
|
||||
GPIOA->MODER |= 2 << (3 * 2);
|
||||
|
||||
GPIOA->AFR[0] &= ~(0xF << (3 * 4));
|
||||
GPIOA->AFR[0] |= 12 << (3 * 4);
|
||||
|
||||
// pull-up
|
||||
GPIOA->PUPDR &= ~(3 << (3 * 2));
|
||||
GPIOA->PUPDR |= 1 << (3 * 2);
|
||||
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||
|
||||
GPIOA->MODER &= ~(3 << (3 * 2));
|
||||
GPIOA->MODER |= 2 << (3 * 2);
|
||||
|
||||
GPIOA->AFR[0] &= ~(0xF << (3 * 4));
|
||||
GPIOA->AFR[0] |= 12 << (3 * 4);
|
||||
|
||||
// pull-up
|
||||
GPIOA->PUPDR &= ~(3 << (3 * 2));
|
||||
GPIOA->PUPDR |= 1 << (3 * 2);
|
||||
}
|
||||
|
||||
void receiver_lpuart_clock_init()
|
||||
{
|
||||
RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
|
||||
RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
|
||||
RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
|
||||
RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
|
||||
RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
|
||||
RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
|
||||
}
|
||||
|
||||
void receiver_uart_init()
|
||||
{
|
||||
receiver_lpuart_clock_init();
|
||||
|
||||
LPUART1->CR1 = 0;
|
||||
LPUART1->CR2 = 0;
|
||||
LPUART1->CR3 = 0;
|
||||
|
||||
LPUART1->BRR = (256 * 16000000UL) / 100000UL;
|
||||
|
||||
// parity control enable
|
||||
LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
|
||||
|
||||
// word length M = 01 - 9 bit
|
||||
LPUART1->CR1 &= ~USART_CR1_M1;
|
||||
LPUART1->CR1 |= USART_CR1_M0;
|
||||
|
||||
// even parity
|
||||
LPUART1->CR1 &= ~USART_CR1_PS;
|
||||
|
||||
// 2 stop bits
|
||||
LPUART1->CR2 &= ~USART_CR2_STOP;
|
||||
LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
|
||||
|
||||
// invertion enabled
|
||||
LPUART1->CR2 |= USART_CR2_RXINV;
|
||||
|
||||
// receiver enable
|
||||
// interrupt generated whenever ORE = 1 or RXNE = 1
|
||||
LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
|
||||
|
||||
// uart enable
|
||||
LPUART1->CR1 |= USART_CR1_UE;
|
||||
|
||||
NVIC_EnableIRQ(LPUART1_IRQn);
|
||||
receiver_lpuart_clock_init();
|
||||
|
||||
LPUART1->CR1 = 0;
|
||||
LPUART1->CR2 = 0;
|
||||
LPUART1->CR3 = 0;
|
||||
|
||||
LPUART1->BRR = (256 * 16000000UL) / 100000UL;
|
||||
|
||||
// parity control enable
|
||||
LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
|
||||
|
||||
// word length M = 01 - 9 bit
|
||||
LPUART1->CR1 &= ~USART_CR1_M1;
|
||||
LPUART1->CR1 |= USART_CR1_M0;
|
||||
|
||||
// even parity
|
||||
LPUART1->CR1 &= ~USART_CR1_PS;
|
||||
|
||||
// 2 stop bits
|
||||
LPUART1->CR2 &= ~USART_CR2_STOP;
|
||||
LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
|
||||
|
||||
// invertion enabled
|
||||
LPUART1->CR2 |= USART_CR2_RXINV;
|
||||
|
||||
// receiver enable
|
||||
// interrupt generated whenever ORE = 1 or RXNE = 1
|
||||
LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
|
||||
|
||||
// uart enable
|
||||
LPUART1->CR1 |= USART_CR1_UE;
|
||||
|
||||
NVIC_EnableIRQ(LPUART1_IRQn);
|
||||
}
|
||||
|
||||
void receiver_init()
|
||||
{
|
||||
receiver_gpio_init();
|
||||
receiver_uart_init();
|
||||
receiver_gpio_init();
|
||||
receiver_uart_init();
|
||||
}
|
||||
|
||||
void LPUART1_IRQHandler()
|
||||
{
|
||||
if (LPUART1->ISR & USART_ISR_RXNE)
|
||||
{
|
||||
uint8_t b = LPUART1->RDR;
|
||||
|
||||
if (b == SBUS_START_BYTE)
|
||||
sbus_index = 0;
|
||||
|
||||
if (sbus_index < SBUS_FRAME_SIZE)
|
||||
sbus_buffer[sbus_index++] = b;
|
||||
|
||||
if (sbus_index == SBUS_FRAME_SIZE)
|
||||
{
|
||||
sbus_index = 0;
|
||||
sbus_frame_ready = 1;
|
||||
}
|
||||
}
|
||||
if (LPUART1->ISR & USART_ISR_RXNE)
|
||||
{
|
||||
uint8_t b = LPUART1->RDR;
|
||||
|
||||
if (b == SBUS_START_BYTE)
|
||||
sbus_index = 0;
|
||||
|
||||
if (sbus_index < SBUS_FRAME_SIZE)
|
||||
sbus_buffer[sbus_index++] = b;
|
||||
|
||||
if (sbus_index == SBUS_FRAME_SIZE)
|
||||
{
|
||||
sbus_index = 0;
|
||||
sbus_frame_ready = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void receiver_update(rc_channels* chs)
|
||||
{
|
||||
if (!sbus_frame_ready)
|
||||
return;
|
||||
|
||||
sbus_frame_ready = 0;
|
||||
|
||||
if (sbus_buffer[0] != SBUS_START_BYTE)
|
||||
return;
|
||||
|
||||
sbus_failsafe = sbus_buffer[23] & (1 << 3);
|
||||
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
|
||||
|
||||
if (sbus_failsafe || sbus_frame_lost)
|
||||
return;
|
||||
|
||||
receiver_parse_frame();
|
||||
|
||||
chs->rc_roll = sbus_channels[0];
|
||||
chs->rc_pitch = sbus_channels[1];
|
||||
chs->rc_throttle = sbus_channels[2];
|
||||
chs->rc_armed = sbus_channels[4];
|
||||
if (!sbus_frame_ready)
|
||||
return;
|
||||
|
||||
sbus_frame_ready = 0;
|
||||
|
||||
if (sbus_buffer[0] != SBUS_START_BYTE)
|
||||
return;
|
||||
|
||||
sbus_failsafe = sbus_buffer[23] & (1 << 3);
|
||||
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
|
||||
|
||||
if (sbus_failsafe || sbus_frame_lost)
|
||||
return;
|
||||
|
||||
receiver_parse_frame();
|
||||
|
||||
chs->rc_roll = sbus_channels[0];
|
||||
chs->rc_pitch = sbus_channels[1];
|
||||
chs->rc_throttle = sbus_channels[2];
|
||||
chs->rc_armed = sbus_channels[4];
|
||||
}
|
||||
|
||||
void receiver_parse_frame()
|
||||
{
|
||||
uint16_t b[22];
|
||||
|
||||
for (uint8_t i = 0; i < 22; ++i)
|
||||
b[i] = sbus_buffer[i + 1];
|
||||
|
||||
sbus_channels[0] = ( b[0] | (b[1] << 8) ) & 0x07FF;
|
||||
sbus_channels[1] = ( (b[1] >> 3) | (b[2] << 5) ) & 0x07FF;
|
||||
sbus_channels[2] = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) ) & 0x07FF;
|
||||
sbus_channels[3] = ( (b[4] >> 1) | (b[5] << 7) ) & 0x07FF;
|
||||
sbus_channels[4] = ( (b[5] >> 4) | (b[6] << 4) ) & 0x07FF;
|
||||
sbus_channels[5] = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) ) & 0x07FF;
|
||||
sbus_channels[6] = ( (b[8] >> 2) | (b[9] << 6) ) & 0x07FF;
|
||||
sbus_channels[7] = ( (b[9] >> 5) | (b[10] << 3) ) & 0x07FF;
|
||||
|
||||
sbus_channels[8] = ( b[11] | (b[12] << 8) ) & 0x07FF;
|
||||
sbus_channels[9] = ( (b[12] >> 3)| (b[13] << 5) ) & 0x07FF;
|
||||
sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
|
||||
sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) ) & 0x07FF;
|
||||
sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) ) & 0x07FF;
|
||||
sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) ) & 0x07FF;
|
||||
sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) ) & 0x07FF;
|
||||
sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) ) & 0x07FF;
|
||||
|
||||
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
|
||||
sbus_failsafe = sbus_buffer[23] & (1 << 3);
|
||||
uint16_t b[22];
|
||||
|
||||
for (uint8_t i = 0; i < 22; ++i)
|
||||
b[i] = sbus_buffer[i + 1];
|
||||
|
||||
sbus_channels[0] = ( b[0] | (b[1] << 8) ) & 0x07FF;
|
||||
sbus_channels[1] = ( (b[1] >> 3) | (b[2] << 5) ) & 0x07FF;
|
||||
sbus_channels[2] = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) ) & 0x07FF;
|
||||
sbus_channels[3] = ( (b[4] >> 1) | (b[5] << 7) ) & 0x07FF;
|
||||
sbus_channels[4] = ( (b[5] >> 4) | (b[6] << 4) ) & 0x07FF;
|
||||
sbus_channels[5] = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) ) & 0x07FF;
|
||||
sbus_channels[6] = ( (b[8] >> 2) | (b[9] << 6) ) & 0x07FF;
|
||||
sbus_channels[7] = ( (b[9] >> 5) | (b[10] << 3) ) & 0x07FF;
|
||||
|
||||
sbus_channels[8] = ( b[11] | (b[12] << 8) ) & 0x07FF;
|
||||
sbus_channels[9] = ( (b[12] >> 3)| (b[13] << 5) ) & 0x07FF;
|
||||
sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
|
||||
sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) ) & 0x07FF;
|
||||
sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) ) & 0x07FF;
|
||||
sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) ) & 0x07FF;
|
||||
sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) ) & 0x07FF;
|
||||
sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) ) & 0x07FF;
|
||||
|
||||
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
|
||||
sbus_failsafe = sbus_buffer[23] & (1 << 3);
|
||||
}
|
||||
|
||||
rc_channels normalize_channels(rc_channels chs)
|
||||
{
|
||||
chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
|
||||
chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
|
||||
chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
|
||||
//chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
|
||||
chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
|
||||
|
||||
return chs;
|
||||
chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
|
||||
chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
|
||||
chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
|
||||
//chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
|
||||
chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
|
||||
|
||||
return chs;
|
||||
}
|
||||
|
||||
int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
|
||||
{
|
||||
return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
|
||||
return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
|
||||
}
|
||||
|
||||
int8_t bool_mapping_gt(int16_t x, int16_t boundary)
|
||||
{
|
||||
return x >= boundary;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void toggle_led()
|
||||
{
|
||||
if (GPIOA->ODR & (1 << 15))
|
||||
{
|
||||
GPIOA->BSRR = 1 << (15 + 16);
|
||||
}
|
||||
else
|
||||
{
|
||||
GPIOA->BSRR = 1 << 15;
|
||||
}
|
||||
}
|
||||
|
||||
void led_init(void)
|
||||
{
|
||||
/* Enable GPIOA clock */
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||
|
||||
/* PA15 -> Output mode */
|
||||
GPIOA->MODER &= ~(3U << (15 * 2));
|
||||
GPIOA->MODER |= (1U << (15 * 2));
|
||||
|
||||
/* Push-pull */
|
||||
GPIOA->OTYPER &= ~(1U << 15);
|
||||
|
||||
/* Low speed (?????????? ??? LED) */
|
||||
GPIOA->OSPEEDR &= ~(3U << (15 * 2));
|
||||
|
||||
/* No pull-up / pull-down */
|
||||
GPIOA->PUPDR &= ~(3U << (15 * 2));
|
||||
|
||||
/* Start with LED OFF */
|
||||
GPIOA->BSRR = (1U << (15 + 16));
|
||||
}
|
||||
return x >= boundary;
|
||||
}
|
||||
Reference in New Issue
Block a user