Skip to content

Commit

Permalink
Support ESC calibration
Browse files Browse the repository at this point in the history
  • Loading branch information
@shengwen-tw
shengwen-tw committed May 25, 2024
1 parent 8d37a11 commit fd5030b
Show file tree
Hide file tree
Showing 4 changed files with 120 additions and 19 deletions.
3 changes: 3 additions & 0 deletions makefiles/dynamics_wizard.mk
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,10 @@ SRC += ./drivers/boards/dynamics_wizard.c
# Tasks
SRC += ./user/tasks/shell_task.c
SRC += ./user/tasks/mavlink_task.c

# Quadrotor application
SRC += ./user/quadrotor/quadrotor.c
SRC += ./user/quadrotor/shell.c

include ./user/navigation/navigation.mk

Expand Down
95 changes: 76 additions & 19 deletions user/quadrotor/quadrotor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,8 @@ static pid_control_t pid_yaw_rate = {
};

static float motors[4] = {0.0f};
static bool flight_ctrl_running;
static bool run_esc_calib;

float calc_elapsed_time(struct timespec *tp_now, struct timespec *tp_last)
{
Expand Down Expand Up @@ -130,14 +132,44 @@ static int rc_safety_check(sbus_t *rc)
return 0;
}

void trigger_esc_calibration(void)
{
run_esc_calib = true;
}

static void esc_calibration_loop(int led_fd, int rc_fd, int pwm_fd)
{
sbus_t rc;
float throttle;

/* Set LED color to purple to indicate the calibration mode */
ioctl(led_fd, LED_R, LED_ENABLE);
ioctl(led_fd, LED_G, LED_DISABLE);
ioctl(led_fd, LED_B, LED_ENABLE);

while (1) {
/* Read RC signal */
read(rc_fd, &rc, sizeof(sbus_t));
throttle = THRUST_PWM_MIN + (rc.throttle * 0.01f) * THRUST_PWM_DIFF;

/* Redirect throttle signal to all motors */
ioctl(pwm_fd, SET_PWM_CHANNEL1, throttle);
ioctl(pwm_fd, SET_PWM_CHANNEL2, throttle);
ioctl(pwm_fd, SET_PWM_CHANNEL3, throttle);
ioctl(pwm_fd, SET_PWM_CHANNEL4, throttle);

/* Yield task for a while */
usleep(1000);
}
}

static void rc_safety_protection(int rc_fd, int led_fd)
{
sbus_t rc;

struct timespec time_last, time_now;
clock_gettime(CLOCK_MONOTONIC, &time_last);

int blink = LED_DISABLE;
ioctl(led_fd, LED_R, LED_DISABLE);
ioctl(led_fd, LED_G, LED_DISABLE);
ioctl(led_fd, LED_B, LED_DISABLE);
Expand All @@ -148,10 +180,13 @@ static void rc_safety_protection(int rc_fd, int led_fd)
clock_gettime(CLOCK_MONOTONIC, &time_now);
if (calc_elapsed_time(&time_now, &time_last) > 100.0f) {
time_last = time_now;
blink = (blink + 1) % 2;
ioctl(led_fd, LED_R, blink);
ioctl(led_fd, LED_R, GPIO_TOGGLE);
}

/* Break the loop if user triggered ESC calibration */
if (run_esc_calib)
break;

/* Update RC signal */
read(rc_fd, &rc, sizeof(sbus_t));

Expand Down Expand Up @@ -251,6 +286,20 @@ static void gyro_bias_estimate(int gyro_fd, float gyro_bias[3])
#endif
}

bool is_flight_ctrl_running(void)
{
return flight_ctrl_running;
}

static void disable_all_motors(int pwm_fd)
{
/* Disable all motors */
ioctl(pwm_fd, SET_PWM_CHANNEL1, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL2, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL3, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL4, THRUST_PWM_MIN);
}

void flight_control_task(void)
{
setprogname("flight control");
Expand Down Expand Up @@ -295,12 +344,6 @@ void flight_control_task(void)
exit(1);
}

/* Initialize thrusts for motor 1 to 4 */
ioctl(pwm_fd, SET_PWM_CHANNEL1, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL2, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL3, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL4, THRUST_PWM_MIN);

/* Open remote control receiver */
int rc_fd = open("/dev/sbus", 0);
if (rc_fd < 0) {
Expand All @@ -314,11 +357,24 @@ void flight_control_task(void)
/* Wait until RC joystick positions are reset */
rc_safety_protection(rc_fd, led_fd);

/* ESC calibration */
if (run_esc_calib)
esc_calibration_loop(led_fd, rc_fd, pwm_fd);

/* Initialize thrusts for motor 1 to 4 */
ioctl(pwm_fd, SET_PWM_CHANNEL1, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL2, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL3, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL4, THRUST_PWM_MIN);

/* Execution time */
struct timespec time_last, time_now;
float time_elapsed = 0.0f;
clock_gettime(CLOCK_MONOTONIC, &time_last);

/* Forbid ESC calibration */
flight_ctrl_running = true;

while (1) {
/* Loop frequency control */
while (time_elapsed < FLIGHT_CTRL_PERIOD) {
Expand Down Expand Up @@ -363,28 +419,29 @@ void flight_control_task(void)

/* Enable motor outputs only if the safety switch is off and
* the throttle is greater than 5% */
if (rc.dual_switch1 || rc.throttle <= 5) {
if (rc.dual_switch1) {
/* Motor disarmed */
ioctl(led_fd, LED_R, LED_DISABLE);
ioctl(led_fd, LED_G, LED_DISABLE);
ioctl(led_fd, LED_B, LED_ENABLE);

/* Disable all motors */
ioctl(pwm_fd, SET_PWM_CHANNEL1, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL2, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL3, THRUST_PWM_MIN);
ioctl(pwm_fd, SET_PWM_CHANNEL4, THRUST_PWM_MIN);
disable_all_motors(pwm_fd);
} else {
/* Motor armed */
ioctl(led_fd, LED_R, LED_ENABLE);
ioctl(led_fd, LED_G, LED_DISABLE);
ioctl(led_fd, LED_B, LED_DISABLE);

/* Set control output */
ioctl(pwm_fd, SET_PWM_CHANNEL1, motors[0]);
ioctl(pwm_fd, SET_PWM_CHANNEL2, motors[1]);
ioctl(pwm_fd, SET_PWM_CHANNEL3, motors[2]);
ioctl(pwm_fd, SET_PWM_CHANNEL4, motors[3]);
if (rc.throttle > 5) {
/* Set control output */
ioctl(pwm_fd, SET_PWM_CHANNEL1, motors[0]);
ioctl(pwm_fd, SET_PWM_CHANNEL2, motors[1]);
ioctl(pwm_fd, SET_PWM_CHANNEL3, motors[2]);
ioctl(pwm_fd, SET_PWM_CHANNEL4, motors[3]);
} else {
disable_all_motors(pwm_fd);
}
}

/* Frequency testing */
Expand Down
9 changes: 9 additions & 0 deletions user/quadrotor/quadrotor.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
#ifndef __QUADROTOR_H__
#define __QUADROTOR_H__

#include <stdbool.h>

bool is_flight_ctrl_running(void);
void trigger_esc_calibration(void);

#endif
32 changes: 32 additions & 0 deletions user/quadrotor/shell.c
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
#include <stdio.h>
#include <string.h>

#include "quadrotor.h"
#include "shell.h"

int esc_calib(int argc, char *argv[])
{
if (is_flight_ctrl_running()) {
shell_puts("command rejected, flight control is running.\n\r");
return 0;
}

/* Confirmation */
struct shell shell;
shell_init_minimal(&shell);
shell_set_prompt(&shell, "confirm esc calibration command [y/n]: ");
shell_listen(&shell);

if (strcmp(shell.buf, "y") == 0 || strcmp(shell.buf, "Y") == 0) {
shell_puts(
"start esc calibration.\n\r"
"restart the system after finishing the calibration.\n\r");
trigger_esc_calibration();
} else {
shell_puts("abort.\n\r");
}

return 0;
}

HOOK_SHELL_CMD("esc_calib", esc_calib);

0 comments on commit fd5030b

Please sign in to comment.