fix setpoint size issue

ZilantRobotics · Nov 27, 2023 · 796b8ba · 796b8ba
1 parent f9aba99
commit 796b8ba
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git a/src/autopilot_interface/cyphal_hitl.cpp b/src/autopilot_interface/cyphal_hitl.cpp
@@ -132,10 +132,12 @@ bool CyphalHitlInterface::get_setpoint(Setpoint16& out_setpoint) {
     prev_setpoint_recv_counter = crnt_setpoint_recv_counter;
 
     auto sp = setpoint.get_setpoint();
-    uint8_t setpoint_size = std::min(setpoint.get_setpoint_size(), (uint8_t)out_setpoint.size());
-    for (size_t sp_idx = 0; sp_idx < setpoint_size; sp_idx++) {
+    for (size_t sp_idx = 0; sp_idx < setpoint.get_setpoint_size(); sp_idx++) {
         out_setpoint[sp_idx] = sp.value[sp_idx];
     }
+    for (size_t sp_idx = setpoint.get_setpoint_size(); sp_idx < out_setpoint.size(); sp_idx++) {
+        out_setpoint[sp_idx] = 0.0;
+    }
 
     return true;
 }

diff --git a/src/params.cpp b/src/params.cpp
@@ -45,7 +45,7 @@ IntegerParamValue_t integer_values_pool[sizeof(integer_desc_pool) / sizeof(Integ
 StringDesc_t string_desc_pool[NUM_OF_STR_PARAMS] = {
     {"name",                            "co.racconlab.cyphal_communicator", IMMUTABLE},
 
-    {"uavcan.sub.setpoint.type",        "reg.udral.service.actuator.common.sp.Vector4", IMMUTABLE},
+    {"uavcan.sub.setpoint.type",        "reg.udral.service.actuator.common.sp.Vector31", IMMUTABLE},
     {"uavcan.sub.readiness.type",       "reg.udral.service.common.Readiness", IMMUTABLE},
     {"uavcan.sub.rgbled.type",          "reg.udral.physics.optics.HighColor", IMMUTABLE},