Merge pull request #196 from Firmament-Autopilot/staging/fmu-v6c

Optimize dshot

Author: JcZou

src/hal/actuator/actuator.h

@@ -81,7 +81,8 @@ struct actuator_device {
 typedef struct actuator_device* actuator_dev_t;
 struct dshot_command {
     rt_uint16_t chan_mask; /* chan_mask: bitmask of channels to target (1<<chan) */
-    rt_uint16_t value;     /* value: DShot command value (0..47) */
+    rt_uint16_t value[16]; /* value: DShot command value (0..47) */
+    rt_size_t size;
     rt_uint8_t repeat;     /* repeat: number of times to send (e.g. 6) */
     rt_uint16_t wait_ms;   /* wait_ms: milliseconds to wait after the full sequence */
 };

src/module/syscmd/cmd_act.c

@@ -15,9 +15,14 @@
  *****************************************************************************/
 #include <firmament.h>
 
+#include "FMS.h"
+#include "hal/actuator/actuator.h"
 #include "module/syscmd/optparse.h"
 #include "module/syscmd/syscmd.h"
 
+MCN_DECLARE(control_output);
+MCN_DECLARE(fms_output);
+
 static void show_usage(void)
 {
     COMMAND_USAGE("act", "<command> [options]");
@@ -48,6 +53,47 @@ 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)
+{
+    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);
+    } 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) {
+                printf("dshot write failed!\n");
+                ret = 0;
+                break;
+                ;
+            }
+
+            systime_msleep(10);
+        }
+    } else {
+        /* Unknown protocol */
+        ret = 0;
+    }
+
+    /* Resume controller output */
+    mcn_resume(MCN_HUB(control_output));
+
+    return ret;
+}
+
 static int set(int argc, struct optparse options)
 {
     char* arg;
@@ -65,6 +111,17 @@ static int set(int argc, struct optparse options)
     uint16_t chan_val[16] = { 0 };
     rt_device_t dev = NULL;
 
+    FMS_Out_Bus fms_out;
+    if (mcn_copy_from_hub(MCN_HUB(fms_output), &fms_out) == FMT_EOK) {
+        if (fms_out.status != VehicleStatus_Disarm) {
+            printf("act set command only allowed when disarmed\n");
+            return EXIT_FAILURE;
+        }
+    } else {
+        printf("failed to get vehicle status\n");
+        return EXIT_FAILURE;
+    }
+
     while ((option = optparse_long(&options, longopts, NULL)) != -1) {
         switch (option) {
         case 'h':
@@ -122,7 +179,7 @@ static int set(int argc, struct optparse options)
             chan_val[i] = val;
         }
 
-        rt_device_write(dev, chan_sel, chan_val, 16);
+        act_device_write(dev, chan_sel, chan_val, 16);
     } else {
         uint8_t index = 0;
         for (int i = 0; i < 16; i++) {
@@ -135,7 +192,7 @@ static int set(int argc, struct optparse options)
             }
         }
 
-        rt_device_write(dev, chan_sel, chan_val, index);
+        act_device_write(dev, chan_sel, chan_val, index);
     }
 
     return EXIT_SUCCESS;

src/module/syscmd/cmd_dshot.c

@@ -11,6 +11,7 @@
 #include "module/syscmd/syscmd.h"
 
 MCN_DECLARE(fms_output);
+MCN_DECLARE(control_output);
 
 static void show_usage(void)
 {
@@ -103,59 +104,87 @@ int cmd_dshot(int argc, char** argv)
     }
 
     actuator_dev_t act = (actuator_dev_t)dev;
+    if (act->config.protocol != ACT_PROTOCOL_DSHOT) {
+        console_printf("device %s protocol is not dshot\n", dev_name);
+        return EXIT_FAILURE;
+    }
+
     chan_sel &= act->chan_mask;
     if (chan_sel == 0) {
         console_printf("no valid channels selected on %s\n", dev_name);
         return EXIT_FAILURE;
     }
 
     struct dshot_command c = { 0 };
+    uint16_t cmd_val = 0;
     c.chan_mask = chan_sel;
+    c.size = 0;
 
     if (STRING_COMPARE(cmd, "reverse")) {
-        c.value = DSHOT_CMD_SPIN_DIRECTION_2;
+        cmd_val = DSHOT_CMD_SPIN_DIRECTION_2;
         c.repeat = 10;
         c.wait_ms = 0;
     } else if (STRING_COMPARE(cmd, "normal")) {
-        c.value = DSHOT_CMD_SPIN_DIRECTION_1;
+        cmd_val = DSHOT_CMD_SPIN_DIRECTION_1;
         c.repeat = 10;
         c.wait_ms = 0;
     } else if (STRING_COMPARE(cmd, "save")) {
-        c.value = DSHOT_CMD_SAVE_SETTINGS;
+        cmd_val = DSHOT_CMD_SAVE_SETTINGS;
         c.repeat = 10;
         c.wait_ms = 35;
     } else if (STRING_COMPARE(cmd, "beep1")) {
-        c.value = DSHOT_CMD_BEEP1;
+        cmd_val = DSHOT_CMD_BEEP1;
         c.repeat = 1;
         c.wait_ms = 400;
     } else if (STRING_COMPARE(cmd, "beep2")) {
-        c.value = DSHOT_CMD_BEEP2;
+        cmd_val = DSHOT_CMD_BEEP2;
         c.repeat = 1;
         c.wait_ms = 400;
     } else if (STRING_COMPARE(cmd, "beep3")) {
-        c.value = DSHOT_CMD_BEEP3;
+        cmd_val = DSHOT_CMD_BEEP3;
         c.repeat = 1;
         c.wait_ms = 400;
     } else if (STRING_COMPARE(cmd, "beep4")) {
-        c.value = DSHOT_CMD_BEEP4;
+        cmd_val = DSHOT_CMD_BEEP4;
         c.repeat = 1;
         c.wait_ms = 400;
     } else if (STRING_COMPARE(cmd, "beep5")) {
-        c.value = DSHOT_CMD_BEEP5;
+        cmd_val = DSHOT_CMD_BEEP5;
         c.repeat = 1;
         c.wait_ms = 400;
     } else {
         console_printf("unknown command: %s\n", cmd);
         return EXIT_FAILURE;
     }
 
-    if (rt_device_control(dev, ACT_CMD_DSHOT_SEND, &c) != RT_EOK) {
-        console_printf("failed to send dshot command\n");
-        return EXIT_FAILURE;
-    } else {
-        console_printf("dshot %s sent to %s channel mask 0x%X\n", cmd, dev_name, c.chan_mask);
+    for (uint16_t i = 0; i < 16; i++) {
+        if (c.chan_mask & (1 << i)) {
+            c.value[i] = cmd_val;
+            c.size++;
+        } else {
+            c.value[i] = 0;
+        }
     }
 
+    /* Suspend controller output */
+    mcn_suspend(MCN_HUB(control_output));
+
+    for (uint16_t i = 0; i < c.repeat; i++) {
+        if (rt_device_control(dev, ACT_CMD_DSHOT_SEND, &c) != RT_EOK) {
+            console_printf("failed to send dshot command\n");
+            /* Resume controller output */
+            mcn_resume(MCN_HUB(control_output));
+            return EXIT_FAILURE;
+        }
+        systime_msleep(5);
+    }
+    systime_msleep(c.wait_ms);
+
+    /* Resume controller output */
+    mcn_resume(MCN_HUB(control_output));
+
+    console_printf("dshot %s sent to %s channel mask 0x%X\n", cmd, dev_name, c.chan_mask);
+
     return EXIT_SUCCESS;
 }
 

target/sieon/s1/board/board.c

@@ -32,7 +32,7 @@
 #include "driver/mag/qmc5883l.h"
 #include "driver/mtd/gd25qxx.h"
 #include "driver/range_finder/tofsense.h"
-#include "drv_actuator.h"
+#include "drv_act.h"
 #include "drv_adc.h"
 #include "drv_eth.h"
 #include "drv_fdcan.h"
@@ -399,7 +399,7 @@ void bsp_early_initialize(void)
     RT_CHECK(drv_spi_init());
 
     /* pwm driver init */
-    RT_CHECK(drv_actuator_init());
+    RT_CHECK(drv_act_init());
 
     /* can driver init */
     RT_CHECK(drv_fdcan_init());