Skip to content

Commit

Permalink
Measure flight control loop frequency
Browse files Browse the repository at this point in the history
  • Loading branch information
@shengwen-tw
shengwen-tw committed May 18, 2024
1 parent 4e7e801 commit 3b8d86a
Show file tree
Hide file tree
Showing 5 changed files with 163 additions and 19 deletions.
11 changes: 9 additions & 2 deletions drivers/boards/bsp_drv.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,15 @@
#define LED2 2
#define LED3 3

#define LED_DISABLE 0
#define LED_ENABLE 1
#define GPIO_PIN0 0

#define GPIO_RESET 0
#define GPIO_SET 1
#define GPIO_TOGGLE 2

#define LED_DISABLE GPIO_RESET
#define LED_ENABLE GPIO_SET
#define LED_TOGGLE GPIO_TOGGLE

void led_write(int fd, int state);

Expand Down
100 changes: 93 additions & 7 deletions drivers/boards/dynamics_wizard.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,18 +19,38 @@ int led_open(struct inode *inode, struct file *file)

int led_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
{
if (arg != LED_ENABLE && arg != LED_DISABLE)
if (arg != LED_ENABLE && arg != LED_DISABLE && arg != LED_TOGGLE)
return -EINVAL;

GPIO_TypeDef *gpio_group;
uint16_t gpio_pin;

/* Pin selection */
switch (cmd) {
case LED0:
GPIO_WriteBit(GPIOA, GPIO_Pin_2, arg);
gpio_group = GPIOA;
gpio_pin = GPIO_Pin_2;
break;
case LED1:
GPIO_WriteBit(GPIOA, GPIO_Pin_0, arg);
gpio_group = GPIOA;
gpio_pin = GPIO_Pin_0;
break;
case LED2:
GPIO_WriteBit(GPIOA, GPIO_Pin_3, arg);
gpio_group = GPIOA;
gpio_pin = GPIO_Pin_3;
break;
default:
return -EINVAL;
}

/* Write new pin state */
switch (arg) {
case LED_ENABLE:
case LED_DISABLE:
GPIO_WriteBit(gpio_group, gpio_pin, arg);
break;
case LED_TOGGLE:
GPIO_ToggleBits(gpio_group, gpio_pin);
break;
default:
return -EINVAL;
Expand All @@ -56,7 +76,8 @@ static void led_init(void)
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_DOWN};
.GPIO_PuPd = GPIO_PuPd_DOWN,
};

GPIO_Init(GPIOA, &GPIO_InitStruct);

Expand All @@ -70,18 +91,83 @@ void led_write(int fd, int state)
ioctl(fd, LED2, state);
}

int gpio_open(struct inode *inode, struct file *file)
{
return 0;
}

int gpio_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
{
if (arg != GPIO_SET && arg != GPIO_RESET && arg != GPIO_TOGGLE)
return -EINVAL;

GPIO_TypeDef *gpio_group;
uint16_t gpio_pin;

/* Pin selection */
switch (cmd) {
case GPIO_PIN0:
gpio_group = GPIOB;
gpio_pin = GPIO_Pin_14;
break;
default:
return -EINVAL;
}

/* Write new pin state */
switch (arg) {
case LED_ENABLE:
case LED_DISABLE:
GPIO_WriteBit(gpio_group, gpio_pin, arg);
break;
case LED_TOGGLE:
GPIO_ToggleBits(gpio_group, gpio_pin);
break;
default:
return -EINVAL;
}

return 0;
}

static struct file_operations gpio_file_ops = {
.ioctl = gpio_ioctl,
.open = gpio_open,
};

static void gpio_init(void)
{
/* Register GPIO to the file system */
register_chrdev("freq_tester", &gpio_file_ops);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

GPIO_InitTypeDef GPIO_InitStruct = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_DOWN,
};

GPIO_Init(GPIOB, &GPIO_InitStruct);

printk("freq_tester: gpio");
}

void early_write(char *buf, size_t size)
{
uart_puts(USART1, buf, size);
}

void __board_init(void)
{
led_init();
pwm_init();
serial1_init(115200, "console", "shell (alias serial0)");
serial3_init(115200, "dbglink", "debug-link (alias serial1)");
// uartX_init(115200, "mavlink", "mavlink (alias serialX)"); //TODO
led_init();
gpio_init();
pwm_init();
sbus_init();
mpu6500_init();
}
27 changes: 23 additions & 4 deletions drivers/boards/stm32f429disc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,34 @@ int led_open(struct inode *inode, struct file *file)

int led_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
{
if (arg != LED_ENABLE && arg != LED_DISABLE)
if (arg != LED_ENABLE && arg != LED_DISABLE && arg != LED_TOGGLE)
return -EINVAL;

GPIO_TypeDef *gpio_group;
uint16_t gpio_pin;

/* Pin selection */
switch (cmd) {
case LED0:
GPIO_WriteBit(GPIOG, GPIO_Pin_13, arg);
gpio_group = GPIOG;
gpio_pin = GPIO_Pin_13;
break;
case LED1:
GPIO_WriteBit(GPIOG, GPIO_Pin_14, arg);
gpio_group = GPIOG;
gpio_pin = GPIO_Pin_14;
break;
default:
return -EINVAL;
}

/* Write new pin state */
switch (arg) {
case LED_ENABLE:
case LED_DISABLE:
GPIO_WriteBit(gpio_group, gpio_pin, arg);
break;
case LED_TOGGLE:
GPIO_ToggleBits(gpio_group, gpio_pin);
break;
default:
return -EINVAL;
Expand Down Expand Up @@ -113,8 +132,8 @@ void __board_init(void)
{
SDRAM_Init();
lcd_init();
led_init();
serial1_init(115200, "console", "shell (alias: serial0)");
serial2_init(115200, "mavlink", "mavlink (alias: serial1)");
serial3_init(115200, "dbglink", "debug-link (alias: serial2)");
led_init();
}
33 changes: 27 additions & 6 deletions drivers/boards/stm32f4disc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,21 +16,42 @@ int led_open(struct inode *inode, struct file *file)

int led_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
{
if (arg != LED_ENABLE && arg != LED_DISABLE)
if (arg != LED_ENABLE && arg != LED_DISABLE && arg != LED_TOGGLE)
return -EINVAL;

GPIO_TypeDef *gpio_group;
uint16_t gpio_pin;

/* Pin selection */
switch (cmd) {
case LED0:
GPIO_WriteBit(GPIOD, GPIO_Pin_12, arg);
gpio_group = GPIOD;
gpio_pin = GPIO_Pin_12;
break;
case LED1:
GPIO_WriteBit(GPIOD, GPIO_Pin_13, arg);
gpio_group = GPIOD;
gpio_pin = GPIO_Pin_13;
break;
case LED2:
GPIO_WriteBit(GPIOD, GPIO_Pin_14, arg);
gpio_group = GPIOD;
gpio_pin = GPIO_Pin_14;
break;
case LED3:
GPIO_WriteBit(GPIOD, GPIO_Pin_15, arg);
gpio_group = GPIOD;
gpio_pin = GPIO_Pin_15;
break;
default:
return -EINVAL;
}

/* Write new pin state */
switch (arg) {
case LED_ENABLE:
case LED_DISABLE:
GPIO_WriteBit(gpio_group, gpio_pin, arg);
break;
case LED_TOGGLE:
GPIO_ToggleBits(gpio_group, gpio_pin);
break;
default:
return -EINVAL;
Expand Down Expand Up @@ -79,8 +100,8 @@ void early_write(char *buf, size_t size)

void __board_init(void)
{
led_init();
serial1_init(115200, "console", "shell (alias: serial0)");
serial2_init(115200, "mavlink", "mavlink (alias: serial1)");
serial3_init(115200, "dbglink", "debug-link (alias: serial2)");
led_init();
}
11 changes: 11 additions & 0 deletions user/tasks/examples/quadrotor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#define LED_R LED0
#define LED_G LED1
#define LED_B LED2
#define FREQ_TESTER GPIO_PIN0

#define deg_to_rad(angle) (angle * M_PI / 180.0)
#define rad_to_deg(radian) (radian * 180.0 / M_PI)
Expand Down Expand Up @@ -241,6 +242,13 @@ void flight_control_task(void)
exit(1);
}

/* Open frequency test pin */
int freq_tester_fd = open("/dev/freq_tester", 0);
if (freq_tester_fd < 0) {
printf("failed to open the frequency tester.\n\r");
exit(1);
}

/* Open Inertial Measurement Unit (IMU) */
int accel_fd = open("/dev/accel0", 0);
int gyro_fd = open("/dev/gyro0", 0);
Expand Down Expand Up @@ -349,6 +357,9 @@ void flight_control_task(void)
ioctl(pwm_fd, SET_PWM_CHANNEL3, motors[2]);
ioctl(pwm_fd, SET_PWM_CHANNEL4, motors[3]);
}

/* Frequency testing */
ioctl(freq_tester_fd, FREQ_TESTER, GPIO_TOGGLE);
}
}

Expand Down

0 comments on commit 3b8d86a

Please sign in to comment.