diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fc90b1..67e5e75 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/autopilot_interface/cyphal_hitl.cpp b/src/autopilot_interface/cyphal_hitl.cpp index f79da88..ed4f9dc 100644 --- a/src/autopilot_interface/cyphal_hitl.cpp +++ b/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() { diff --git a/src/autopilot_interface/cyphal_hitl.hpp b/src/autopilot_interface/cyphal_hitl.hpp index 23a4f0a..0757a58 100644 --- a/src/autopilot_interface/cyphal_hitl.hpp +++ b/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(); diff --git a/src/main.cpp b/src/main.cpp index ca5d7a7..781d211 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,7 +22,7 @@ static std::unique_ptr 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(); diff --git a/src/simulator_interface/ap_json/ap_json.cpp b/src/simulator_interface/ap_json/ap_json.cpp index 83e7804..b57fb33 100644 --- a/src/simulator_interface/ap_json/ap_json.cpp +++ b/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 servo_pwm; for (size_t sp_idx = 0; sp_idx < 4; sp_idx++) { servo_pwm[sp_idx] = 1000 + 1000.0 * setpoint[sp_idx]; diff --git a/src/simulator_interface/ap_json/ap_json.hpp b/src/simulator_interface/ap_json/ap_json.hpp index 619fabe..2e02bfa 100644 --- a/src/simulator_interface/ap_json/ap_json.hpp +++ b/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: diff --git a/src/simulator_interface/ros_interface/ros_interface.cpp b/src/simulator_interface/ros_interface/ros_interface.cpp index d8b1fcc..ad536c4 100644 --- a/src/simulator_interface/ros_interface/ros_interface.cpp +++ b/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); diff --git a/src/simulator_interface/ros_interface/ros_interface.hpp b/src/simulator_interface/ros_interface/ros_interface.hpp index e7e77ec..95a110b 100644 --- a/src/simulator_interface/ros_interface/ros_interface.hpp +++ b/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: diff --git a/src/simulator_interface/simulator_interface.hpp b/src/simulator_interface/simulator_interface.hpp index ccde82c..6e9a3cb 100644 --- a/src/simulator_interface/simulator_interface.hpp +++ b/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;}