fix setpoint size

CMakeLists.txt

@@ -79,11 +79,13 @@ add_custom_target(
   compile_dsdl ALL
   COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/compile_dsdl.sh
   COMMENT "Compile DSDL"
+  OUTPUT_QUIET
 )
 add_custom_target(
   gen_headers ALL
   COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/Libs/cyphal_application/scripts/nnvg_generate_c_headers.sh
   COMMENT "Generate C headers"
+  OUTPUT_QUIET
 )
 add_dependencies(${PROJECT_NAME} compile_dsdl)
 add_dependencies(${PROJECT_NAME} gen_headers)

src/autopilot_interface/cyphal_hitl.cpp

@@ -123,11 +123,11 @@ void CyphalHitlInterface::publish_rangefinder(float range) {
     rangefinder.publish(range);
 }
 
-bool CyphalHitlInterface::get_setpoint(Setpoint16& out_setpoint) {
+size_t CyphalHitlInterface::get_setpoint(Setpoint16& out_setpoint) {
     static uint32_t prev_setpoint_recv_counter = 0;
     uint32_t crnt_setpoint_recv_counter = setpoint.get_recv_counter();
     if (crnt_setpoint_recv_counter == prev_setpoint_recv_counter) {
-        return false;
+        return 0;
     }
     prev_setpoint_recv_counter = crnt_setpoint_recv_counter;
 
@@ -139,7 +139,7 @@ bool CyphalHitlInterface::get_setpoint(Setpoint16& out_setpoint) {
         out_setpoint[sp_idx] = 0.0;
     }
 
-    return true;
+    return setpoint.get_setpoint_size();
 }
 
 bool CyphalHitlInterface::get_arming_status() {

src/autopilot_interface/cyphal_hitl.hpp

@@ -59,7 +59,7 @@ class CyphalHitlInterface {
     void publish_magnetometer(const Vector3 magnetic_field_gauss);
     void publish_rangefinder(float range);
 
-    bool get_setpoint(Setpoint16& setpoint);
+    size_t get_setpoint(Setpoint16& setpoint);
     bool get_arming_status();
     uint32_t get_setpoint_recv_counter();
     void clear_servo_pwm_counter();

src/main.cpp

@@ -22,7 +22,7 @@ static std::unique_ptr<SimulatorBaseInterface> init_sim_interface(int argc, char
         std::cout << "ArduPilot Initialization Error." << std::endl;
     }
     Setpoint16 setpoint;
-    sim->send_setpoint(setpoint);
+    sim->send_setpoint(setpoint, 16);
 
     std::cout << "Hello, ArduPilot JSON." << std::endl;
 
@@ -87,8 +87,9 @@ int main(int argc, char** argv) {
         cyphal_hitl.process();
 
         Setpoint16 setpoint;
-        if (cyphal_hitl.get_setpoint(setpoint)) {
-            simulator->send_setpoint(setpoint);
+        auto setpoint_size = cyphal_hitl.get_setpoint(setpoint);
+        if (setpoint_size) {
+            simulator->send_setpoint(setpoint, setpoint_size);
         }
 
         uint32_t crnt_time_ms = HAL_GetTick();

src/simulator_interface/ap_json/ap_json.cpp

@@ -50,7 +50,9 @@ bool ArdupilotJsonInterface::init() {
     return true;
 }
 
-bool ArdupilotJsonInterface::send_setpoint(const Setpoint16& setpoint) {
+bool ArdupilotJsonInterface::send_setpoint(const Setpoint16& setpoint, size_t size) {
+    (void)size;
+
     std::array<uint16_t, 16> servo_pwm;
     for (size_t sp_idx = 0; sp_idx < 4; sp_idx++) {
         servo_pwm[sp_idx] = 1000 + 1000.0 * setpoint[sp_idx];

src/simulator_interface/ap_json/ap_json.hpp

@@ -24,7 +24,7 @@ class ArdupilotJsonInterface : public SimulatorBaseInterface{
 public:
     ArdupilotJsonInterface(double home_lat, double home_lon, double home_alt);
     bool init() override;
-    bool send_setpoint(const Setpoint16& setpoint) override;
+    bool send_setpoint(const Setpoint16& setpoint, size_t size) override;
     bool spin_once() override;
 
 private:

src/simulator_interface/ros_interface/ros_interface.cpp

@@ -45,10 +45,10 @@ bool RosInterface::init() {
     return true;
 }
 
-bool RosInterface::send_setpoint(const Setpoint16& setpoint) {
+bool RosInterface::send_setpoint(const Setpoint16& setpoint, size_t size) {
     sensor_msgs::Joy ros_msg;
     ros_msg.header.stamp = ros::Time::now();
-    for (uint_fast8_t idx = 0; idx < 16; idx++) {
+    for (uint_fast8_t idx = 0; idx < size; idx++) {
         ros_msg.axes.push_back(std::clamp(setpoint[idx], -1.0f, +1.0f));
     }
     _setpoint_pub.publish(ros_msg);

src/simulator_interface/ros_interface/ros_interface.hpp

@@ -31,7 +31,7 @@ class RosInterface : public SimulatorBaseInterface{
 public:
     RosInterface(int argc, char** argv);
     bool init() override;
-    bool send_setpoint(const Setpoint16& setpoint) override;
+    bool send_setpoint(const Setpoint16& setpoint, size_t size) override;
     void send_arming_status(bool arming_status) override;
     bool spin_once() override;
 private:

src/simulator_interface/simulator_interface.hpp

@@ -44,7 +44,7 @@ class SimulatorBaseInterface{
     /**
      * @note You send should send setpoint with at least 200 Hz frequency
      */
-    virtual bool send_setpoint(const Setpoint16& setpoint) = 0;
+    virtual bool send_setpoint(const Setpoint16& setpoint, size_t size) = 0;
 
     virtual void send_arming_status(bool armings_status) {(void)armings_status;}