From 459672d02f272ad6b31c8f1002cb4e1a47da7774 Mon Sep 17 00:00:00 2001
From: Shengwen Cheng <shengwen1997.tw@gmail.com>
Date: Sun, 26 May 2024 15:04:15 +0800
Subject: [PATCH] Fix precision loss of PWM setting

---
 drivers/periph/pwm.c       | 12 +++++++--
 user/quadrotor/quadrotor.c | 52 ++++++++++++++++++++------------------
 2 files changed, 38 insertions(+), 26 deletions(-)

diff --git a/drivers/periph/pwm.c b/drivers/periph/pwm.c
index 78e87c1..fed23c2 100644
--- a/drivers/periph/pwm.c
+++ b/drivers/periph/pwm.c
@@ -1,3 +1,4 @@
+#include <errno.h>
 #include <stdint.h>
 
 #include <fs/fs.h>
@@ -11,7 +12,8 @@
 #define MICROSECOND 1000000
 
 /* Map from +width time to timer reload value */
-#define MICROSEC_TO_RELOAD(us) (us * (PWM_RELOAD * PWM_FREQ / MICROSECOND))
+#define MICROSEC_TO_RELOAD(us) \
+    (us * ((float) PWM_RELOAD * PWM_FREQ / MICROSECOND))
 
 int pwm_open(struct inode *inode, struct file *file)
 {
@@ -20,7 +22,13 @@ int pwm_open(struct inode *inode, struct file *file)
 
 int pwm_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
-    unsigned long reload = MICROSEC_TO_RELOAD(arg);
+    float width_microsecond = *(float *) arg;
+    uint32_t reload = (uint32_t) MICROSEC_TO_RELOAD(width_microsecond);
+
+    if (reload > PWM_RELOAD) {
+        printk("ioctl(): pwm setting exceeded maximum value");
+        return -EINVAL;
+    }
 
     switch (cmd) {
     case SET_PWM_CHANNEL1:
diff --git a/user/quadrotor/quadrotor.c b/user/quadrotor/quadrotor.c
index 6081323..457b5f0 100644
--- a/user/quadrotor/quadrotor.c
+++ b/user/quadrotor/quadrotor.c
@@ -115,6 +115,27 @@ void bound_float(float *val, float max, float min)
         *val = min;
 }
 
+static void set_motor_outputs(int pwm_fd, float motors[4])
+{
+    ioctl(pwm_fd, SET_PWM_CHANNEL1, (unsigned long) &motors[0]);
+    ioctl(pwm_fd, SET_PWM_CHANNEL2, (unsigned long) &motors[1]);
+    ioctl(pwm_fd, SET_PWM_CHANNEL3, (unsigned long) &motors[2]);
+    ioctl(pwm_fd, SET_PWM_CHANNEL4, (unsigned long) &motors[3]);
+}
+
+static void disable_all_motors(int pwm_fd)
+{
+    float motors[4];
+    motors[0] = THRUST_PWM_MIN;
+    motors[1] = THRUST_PWM_MIN;
+    motors[2] = THRUST_PWM_MIN;
+    motors[3] = THRUST_PWM_MIN;
+    ioctl(pwm_fd, SET_PWM_CHANNEL1, (unsigned long) &motors[0]);
+    ioctl(pwm_fd, SET_PWM_CHANNEL2, (unsigned long) &motors[1]);
+    ioctl(pwm_fd, SET_PWM_CHANNEL3, (unsigned long) &motors[2]);
+    ioctl(pwm_fd, SET_PWM_CHANNEL4, (unsigned long) &motors[3]);
+}
+
 static int rc_safety_check(sbus_t *rc)
 {
     if (rc->dual_switch1 == false)
@@ -140,6 +161,7 @@ static void esc_calibration_loop(int led_fd, int rc_fd, int pwm_fd)
 {
     sbus_t rc;
     float throttle;
+    float motors[4];
 
     /* Set LED color to purple to indicate the calibration mode */
     ioctl(led_fd, LED_R, LED_ENABLE);
@@ -152,10 +174,11 @@ static void esc_calibration_loop(int led_fd, int rc_fd, int pwm_fd)
         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);
+        motors[0] = throttle;
+        motors[1] = throttle;
+        motors[2] = throttle;
+        motors[3] = throttle;
+        set_motor_outputs(pwm_fd, motors);
 
         /* Yield task for a while */
         usleep(1000);
@@ -290,22 +313,6 @@ bool is_flight_ctrl_running(void)
     return flight_ctrl_running;
 }
 
-static void set_motor_outputs(int pwm_fd, float motors[4])
-{
-    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]);
-}
-
-static void disable_all_motors(int pwm_fd)
-{
-    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");
@@ -368,10 +375,7 @@ void flight_control_task(void)
         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);
+    disable_all_motors(pwm_fd);
 
     /* Execution time */
     struct timespec time_last, time_now;