Optimize dshot

Author: JcZou

src/hal/actuator/actuator.c

@@ -133,13 +133,7 @@ uint16_t dshot_pack_frame(uint16_t value, bool telemetry)
 
 uint16_t dshot_throttle_to_value(float norm_throttle)
 {
-    if (norm_throttle < 0.0f) {
-        norm_throttle = 0.0f;
-    } else if (norm_throttle > 1.0f) {
-        norm_throttle = 1.0f;
-    }
-
-    return (uint16_t)(DSHOT_MIN_THROTTLE + (uint16_t)(norm_throttle * DSHOT_RANGE));
+    return (uint16_t)(DSHOT_MIN_THROTTLE + (uint16_t)(constrain_float(norm_throttle, 0.0f, 1.0f) * DSHOT_RANGE));
 }
 
 /**

src/module/syscmd/cmd_act.c

@@ -53,28 +53,25 @@ static void show_get_usage(void)
     SHELL_OPTION("-c, --channel", "Selected channel in hex, e.g, f means channel 1 to channel 4.");
 }
 
-static rt_size_t act_device_write(rt_device_t dev, rt_uint16_t chan_sel, const rt_uint16_t* chan_val, rt_size_t size)
+static rt_size_t act_device_write(actuator_dev_t act_dev, rt_uint16_t chan_sel, const rt_uint16_t* chan_val, rt_size_t size)
 {
-    actuator_dev_t act_dev = (actuator_dev_t)dev;
     rt_size_t ret = size;
 
     /* Suspend controller output */
     mcn_suspend(MCN_HUB(control_output));
 
     if (act_dev->config.protocol == ACT_PROTOCOL_PWM) {
-        ret = rt_device_write(dev, chan_sel, chan_val, size);
+        ret = rt_device_write(&act_dev->parent, chan_sel, chan_val, size);
     } else if (act_dev->config.protocol == ACT_PROTOCOL_DSHOT) {
-
         printf("Press any key to stop...\n");
-
         while (1) {
             /* type any key to exit */
             if (syscmd_has_input()) {
                 syscmd_flush();
                 break;
             }
 
-            if (rt_device_write(dev, chan_sel, chan_val, size) != size) {
+            if (rt_device_write(&act_dev->parent, chan_sel, chan_val, size) != size) {
                 printf("dshot write failed!\n");
                 ret = 0;
                 break;
@@ -109,7 +106,7 @@ static int set(int argc, struct optparse options)
     uint8_t all = 0;
     uint16_t chan_sel = 0;
     uint16_t chan_val[16] = { 0 };
-    rt_device_t dev = NULL;
+    actuator_dev_t dev = NULL;
 
     FMS_Out_Bus fms_out;
     if (mcn_copy_from_hub(MCN_HUB(fms_output), &fms_out) == FMT_EOK) {
@@ -132,7 +129,7 @@ static int set(int argc, struct optparse options)
             chan_sel = 0xFFFF;
             break;
         case 'd':
-            dev = rt_device_find(options.optarg);
+            dev = (actuator_dev_t)rt_device_find(options.optarg);
             if (dev == NULL) {
                 printf("Can not find device:%s\n", options.optarg);
                 return EXIT_FAILURE;
@@ -154,6 +151,7 @@ static int set(int argc, struct optparse options)
 
     int arg_c = argc - options.optind;
     int chan_num = 0;
+    uint16_t val;
 
     for (int i = 0; i < 16; i++) {
         if (chan_sel & (1 << i)) {
@@ -167,7 +165,6 @@ static int set(int argc, struct optparse options)
     }
 
     if (all) {
-        uint16_t val;
         arg = optparse_arg(&options);
         if (arg == NULL) {
             show_set_usage();
@@ -176,6 +173,10 @@ static int set(int argc, struct optparse options)
         val = atoi(arg);
 
         for (int i = 0; i < 16; i++) {
+            if (val < dev->range[0] || val > dev->range[1]) {
+                printf("Invalid value! The allowed range is [%d, %d]\n", dev->range[0], dev->range[1]);
+                return EXIT_FAILURE;
+            }
             chan_val[i] = val;
         }
 
@@ -188,7 +189,13 @@ static int set(int argc, struct optparse options)
                     show_set_usage();
                     return EXIT_FAILURE;
                 }
-                chan_val[index++] = atoi(arg);
+
+                val = atoi(arg);
+                if (val < dev->range[0] || val > dev->range[1]) {
+                    printf("the allowed range is [%d, %d]\n", dev->range[0], dev->range[1]);
+                    return EXIT_FAILURE;
+                }
+                chan_val[index++] = val;
             }
         }
 

target/sieon/s1/drivers/drv_act.c

@@ -853,7 +853,7 @@ rt_size_t main_act_write(actuator_dev_t dev, rt_uint16_t chan_sel, const rt_uint
                     /* Handle dshot command */
                     dshot_val = val;
                 } else {
-                    float norm_throttle = (val > 1000) ? ((float)(val - 1000) / 1000.0f) : 0.0f;
+                    float norm_throttle = (float)(val - 1000) / 1000.0f;
                     dshot_val = dshot_throttle_to_value(norm_throttle);
                 }