upd arming logic, add is_engaged param to pwm channels, upd Dronecan …

…to one with ArmingStatus
RaccoonlabDev · Sep 2, 2024 · c429303 · c429303
1 parent 310dd36
commit c429303
Show file tree

Hide file tree

Showing 6 changed files with 48 additions and 56 deletions.
diff --git a/Libs/Dronecan b/Libs/Dronecan
diff --git a/Src/modules/dronecan/arming/arming.cpp b/Src/modules/dronecan/arming/arming.cpp
@@ -13,52 +13,30 @@
 
 Logger logger = Logger("ArmingModule");
 
-REGISTER_MODULE(ArmingModule)
+
 bool ArmingModule::is_armed = false;
 uint32_t ArmingModule::arm_start_time = 0;
-int64_t ArmingModule::disarm_time = -1;
+
+REGISTER_MODULE(ArmingModule)
 
 void ArmingModule::init() {
     health = Status::OK;
-
-    if (raw_command_sub.init(raw_command_cb) < 0) {
-        health = Status::FATAL_MALFANCTION;
-    }
     prev_eng_time = paramsGetIntegerValue(IntParamsIndexes::PARAM_STATS_ENG_TIME);
-
     mode = Module::Mode::STANDY;
 }
 
 void ArmingModule::spin_once() {
-    if (!is_armed && disarm_time > 0) {
-        auto cur_eng_time = (HAL_GetTick() - ArmingModule::arm_start_time) / 1000;
-        prev_eng_time = cur_eng_time + prev_eng_time;
-        paramsSetIntegerValue(IntParamsIndexes::PARAM_STATS_ENG_TIME, int(prev_eng_time));
-        paramsSave();
-        disarm_time = -1;
-    }
-}
-
-void ArmingModule::raw_command_cb(const RawCommand_t& msg) {
-    if (msg.size < RAW_COMMAND_CHANNEL + 1) {
-        return;
-    }
-    auto raw_command_value = msg.raw_cmd[RAW_COMMAND_CHANNEL];
-
-    if (!is_armed) {
-        if (raw_command_value > -1){
-            is_armed = true;
-            if (disarm_time != -1) {
-                disarm_time = -1;
-                arm_start_time = HAL_GetTick();
-            }
-        }
+    global_mode = ModuleManager::get_global_mode();
+    if (global_mode == Mode::ENGAGED && !is_armed) {
+        is_armed = true;
+        arm_start_time = HAL_GetTick();
         return;
     }
-
-    // armed
-    if (is_armed && raw_command_value < 0) {
-        disarm_time = HAL_GetTick();
+    if (is_armed && global_mode != Mode::ENGAGED) {
         is_armed = false;
+        auto cur_eng_time = (HAL_GetTick() - arm_start_time) / 1000;
+        prev_eng_time = cur_eng_time + prev_eng_time;
+        paramsSetIntegerValue(IntParamsIndexes::PARAM_STATS_ENG_TIME, int(prev_eng_time));
+        paramsSave();
     }
 }
diff --git a/Src/modules/dronecan/arming/arming.hpp b/Src/modules/dronecan/arming/arming.hpp
@@ -16,20 +16,17 @@ extern "C" {
 
 class ArmingModule : public Module {
 public:
-    ArmingModule() : Module(0.2) {}
+    ArmingModule() : Module(2, Protocol::DRONECAN) {}
     void init() override;
 
 protected:
     void spin_once() override;
 
 private:
-    static void raw_command_cb(const RawCommand_t& msg);
-    static inline DronecanSubscriber<RawCommand_t> raw_command_sub;
+    Mode global_mode;
     static bool is_armed;
     static uint32_t arm_start_time;
     uint32_t prev_eng_time = 0;
-    static int64_t disarm_time;
-    static constexpr uint8_t RAW_COMMAND_CHANNEL = 0;
 };
 
 #ifdef __cplusplus

diff --git a/Src/modules/dronecan/pwm/PWMModule.cpp b/Src/modules/dronecan/pwm/PWMModule.cpp
@@ -32,7 +32,6 @@ uint16_t PWMModule::ttl_cmd = 500;
 uint16_t PWMModule::pwm_freq = 50;
 CommandType PWMModule::pwm_cmd_type = CommandType::RAW_COMMAND;
 
-REGISTER_MODULE(PWMModule)
 
 std::array<PwmChannelInfo, static_cast<uint8_t>(HAL::PwmPin::PWM_AMOUNT)> PWMModule::params = {{
     {{.min = MIN(1), .max = MAX(1), .def = DEF(1), .ch = CH(1), .fb = FB(1)}, HAL::PwmPin::PWM_1},
@@ -41,6 +40,8 @@ std::array<PwmChannelInfo, static_cast<uint8_t>(HAL::PwmPin::PWM_AMOUNT)> PWMMod
     {{.min = MIN(4), .max = MAX(4), .def = DEF(4), .ch = CH(4), .fb = FB(4)}, HAL::PwmPin::PWM_4},
 }};
 
+REGISTER_MODULE(PWMModule)
+
 void PWMModule::init() {
     mode = Module::Mode::INITIALIZATION;
 
@@ -63,13 +64,13 @@ void PWMModule::spin_once() {
     for (auto& pwm : params) {
         if (crnt_time_ms > pwm.cmd_end_time_ms) {
             pwm.command_val = pwm.def;
+            pwm.is_engaged = false;
         }
         HAL::Pwm::set_duration(pwm.pin, pwm.command_val);
-        if (pwm.command_val != pwm.def) {
-            mode = Module::Mode::ENGAGED;
+        if (pwm.is_engaged) {
+            mode = Mode::ENGAGED;
         }
     }
-
     status_pub_timeout_ms = 1;
     static uint32_t next_pub_ms = 5000;
 
@@ -234,10 +235,14 @@ void PWMModule::raw_command_cb(const RawCommand_t& msg) {
     }
 
     for (auto& pwm : params) {
-        if (pwm.channel >= msg.size) {
+        if (pwm.channel >= msg.size || pwm.channel < 0) {
             continue;
         }
-
+        if (msg.raw_cmd[pwm.channel] > -1) {
+            pwm.is_engaged = true;
+        } else {
+            pwm.is_engaged = false;
+        }
         pwm.cmd_end_time_ms = HAL_GetTick() + ttl_cmd;
         pwm.command_val = mapInt16CommandToPwm(msg.raw_cmd[pwm.channel], pwm.min, pwm.max, pwm.def);
     }
@@ -262,6 +267,7 @@ void PWMModule::array_command_callback(CanardRxTransfer* transfer) {
             if (command.commads[j].actuator_id != pwm->channel) {
                 continue;
             }
+            pwm->is_engaged = true;
             pwm->cmd_end_time_ms = HAL_GetTick() + ttl_cmd;
             pwm->command_val = mapFloatCommandToPwm(command.commads[j].command_value,
                                                     pwm->min, pwm->max, pwm->def);
@@ -283,9 +289,15 @@ void PWMModule::hardpoint_callback(CanardRxTransfer* transfer) {
         if (cmd.hardpoint_id != pwm.channel) {
             continue;
         }
-
+        pwm.is_engaged = true;
         // TTL has no effect on Hardpoint
         pwm.cmd_end_time_ms = std::numeric_limits<decltype(pwm.cmd_end_time_ms)>::max();
         pwm.command_val = (cmd.command == CMD_HOLD_OR_MAX) ? pwm.max : pwm.min;
     }
 }
+
+void PWMModule::arming_status_callback(const ArmingStatus& msg) {
+    if (msg == ArmingStatus::STATUS_FULLY_ARMED) {
+        // params[0].is_engaged = true;
+    }
+}
diff --git a/Src/modules/dronecan/pwm/PWMModule.hpp b/Src/modules/dronecan/pwm/PWMModule.hpp
@@ -38,21 +38,22 @@ struct PwmChannelsParamsNames {
 struct PwmChannelInfo {
     const PwmChannelsParamsNames names;
     HAL::PwmPin pin;
-    uint16_t min{0};
-    uint16_t max{0};
-    uint16_t def{0};
-    int16_t channel{-1};
-    uint16_t command_val{0};
-    uint32_t cmd_end_time_ms{0};
-    uint32_t next_status_pub_ms{0};
-    uint8_t fb{0};
+    uint16_t    min{0};
+    uint16_t    max{0};
+    uint16_t    def{0};
+    int16_t     channel{-1};
+    uint16_t    command_val{0};
+    uint32_t    cmd_end_time_ms{0};
+    uint32_t    next_status_pub_ms{0};
+    uint8_t     fb{0};
+    bool        is_engaged{false};
 };
 
 class PWMModule : public Module {
 public:
     void init() override;
 
-    inline PWMModule() : Module(50) {}
+    inline PWMModule() : Module(50, Protocol::DRONECAN) {}
 
 protected:
     void update_params() override;
@@ -70,6 +71,7 @@ class PWMModule : public Module {
     static void raw_command_cb(const RawCommand_t& msg);
     static void array_command_callback(CanardRxTransfer* transfer);
     static void hardpoint_callback(CanardRxTransfer* transfer);
+    static void arming_status_callback(const ArmingStatus& msg);
 
     static void publish_esc_status();
     static void publish_actuator_status();

diff --git a/Src/platform/ubuntu/adc.cpp b/Src/platform/ubuntu/adc.cpp
@@ -18,6 +18,7 @@ int8_t AdcPeriphery::init() {
     state.temperature = 3115;
     state.current = 2048;
     state.v_in = 640;
+    state.v_5v = 3200;
     _is_adc_already_inited = true;
     return 0;
 }
@@ -31,6 +32,8 @@ uint16_t AdcPeriphery::get(AdcChannel channel) {
         return state.current;
     case AdcChannel::ADC_TEMPERATURE:
         return state.temperature;
+    case AdcChannel::ADC_5V:
+        return state.v_5v;
     default:
         break;
     }
+4 −0		include/serialization/subscriber.hpp
+48 −0		include/serialization/uavcan/equipment/safety/ArmingStatus.h