Connection and Communication Initialization issue with Simulink model(2024b) and Ardupilot through SITL

I have imported a 3D CAD model of a quadcopter into Simulink Multibody and am working to connect the Simulink model to ArduPilot using SITL. To achieve this, I am using an S-Function block that is designed to send input data from Simulink to ArduPilot and receive output data from ArduPilot back into Simulink, forming a closed-loop system.

The model compiles and runs without errors, but it fails to establish a connection or communication with ArduPilot. When I hit the run button, the simulation starts immediately without waiting for ArduPilot to connect. I’m unable to identify why this is happening. The code for my S-Function is attached. Could someone please help me troubleshoot this issue?

#define S_FUNCTION_NAME sil_connector_new1
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"
#include <iostream>
#include <winsock2.h>
#include <Ws2tcpip.h>
#include "C:/Users/tihan/Desktop/ardupilot/build/sitl/libraries/GCS_MAVLink/include/mavlink/v2.0/ardupilotmega/mavlink.h"

#pragma comment(lib, "ws2_32.lib")

#define SERVER_IP "127.0.0.1"
#define SERVER_PORT 5760

class UDPConnector {
public:
UDPConnector(const char* ip, int port) : server_ip(ip), server_port(port), sockfd(-1) {}

bool initialize() {
WSADATA wsaData;
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
std::cerr << "WSAStartup failed with error: " << WSAGetLastError() << std::endl;
return false;
}

sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd == INVALID_SOCKET) {
std::cerr << "Socket creation failed with error: " << WSAGetLastError() << std::endl;
return false;
}

memset(&server_addr, 0, sizeof(server_addr));
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(server_port);
server_addr.sin_addr.s_addr = inet_addr(server_ip);

// Try to connect to SITL
std::cout << "Waiting for ArduPilot SITL connection…" << std::endl;
for (int attempt = 0; attempt < 10; ++attempt) {
if (test_connection()) {
std::cout << "Connection established to ArduPilot SITL on " << server_ip << ":" << server_port << std::endl;
return true;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}

std::cerr << "Failed to establish connection to ArduPilot SITL." << std::endl;
return false;
}

bool send_message(const uint8_t* buffer, size_t length) {
if (sendto(sockfd, (const char*)buffer, length, 0, (struct sockaddr*)&server_addr, sizeof(server_addr)) == SOCKET_ERROR) {
std::cerr << "Failed to send message with error: " << WSAGetLastError() << std::endl;
return false;
}
return true;
}

~UDPConnector() {
if (sockfd != INVALID_SOCKET) {
closesocket(sockfd);
}
WSACleanup();
}

private:
const char* server_ip;
int server_port;
SOCKET sockfd;
struct sockaddr_in server_addr;

bool test_connection() {
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Send a heartbeat message to test the connection
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_ONBOARD_CONTROLLER, MAV_AUTOPILOT_INVALID, 0, 0, 0);
size_t len = mavlink_msg_to_send_buffer(buf, &msg);

if (!send_message(buf, len)) {
return false;
}

// Non-blocking receive to verify response
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sockfd, &readfds);

struct timeval timeout;
timeout.tv_sec = 1;
timeout.tv_usec = 0;

if (select(0, &readfds, NULL, NULL, &timeout) > 0) {
char recv_buf[1024];
int recv_len = recvfrom(sockfd, recv_buf, sizeof(recv_buf), 0, NULL, NULL);
if (recv_len > 0) {
std::cout << "Heartbeat received from SITL." << std::endl;
return true;
}
}

return false;
}
};

UDPConnector connector(SERVER_IP, SERVER_PORT);

void wait_for_ardupilot_connection(SimStruct *S) {
// Wait for connection with ArduPilot
if (!connector.initialize()) {
ssSetErrorStatus(S, "Unable to establish UDP connection to ArduPilot SITL.");
return;
}
}

void mdlInitializeSizes(SimStruct *S) {
ssSetNumInputPorts(S, 7); // 7 inputs: 6 vectors + 1 constant (time)
ssSetNumOutputPorts(S, 4); // 4 outputs for the 4 motor PWM signals

// 6 input ports for sensor data as [1×3] vectors
for (int i = 0; i < 6; ++i) { // 6 inputs for acceleration, gyro, position, velocity, attitude, magnetometer
ssSetInputPortWidth(S, i, 3); // Each sensor input is a [1×3] vector
ssSetInputPortDirectFeedThrough(S, i, 1); // Declare direct feedthrough
}

// Time input (constant scalar)
ssSetInputPortWidth(S, 6, 1); // Time as constant (scalar)
ssSetInputPortDirectFeedThrough(S, 6, 1); // Declare direct feedthrough for time input

// 4 output ports for motor PWM signals
for (int i = 0; i < 4; ++i) {
ssSetOutputPortWidth(S, i, 1); // Output for each motor PWM signal
}

ssSetNumSampleTimes(S, 1);
}

void mdlInitializeSampleTimes(SimStruct *S) {
ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
ssSetOffsetTime(S, 0, 0.0);
}

void mdlStart(SimStruct *S) {
// Initialize the UDP connection to ArduPilot SITL
wait_for_ardupilot_connection(S);
}

void send_mavlink_command(SimStruct *S, uint8_t command, float param1, float param2, float param3) {
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];

// Construct the COMMAND_LONG message
mavlink_msg_command_long_pack(1, 200, &msg, 1, 1, command, 0, param1, param2, param3, 0, 0, 0, 0);

// Send the command to ArduPilot via UDP
uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg);
connector.send_message(buffer, len);
}

void mdlOutputs(SimStruct *S, int_T tid) {
const real_T *accel = ssGetInputPortRealSignal(S, 0); // [1×3] vector for acceleration
const real_T *gyro = ssGetInputPortRealSignal(S, 1); // [1×3] vector for gyro data
const real_T *position = ssGetInputPortRealSignal(S, 2); // [1×3] vector for position
const real_T *velocity = ssGetInputPortRealSignal(S, 3); // [1×3] vector for velocity
const real_T *attitude = ssGetInputPortRealSignal(S, 4); // [1×3] vector for attitude (phi, theta, psi)
const real_T *mag = ssGetInputPortRealSignal(S, 5); // [1×3] vector for magnetometer data
const real_T *timestamp = ssGetInputPortRealSignal(S, 6); // Scalar for time input (constant)

// Send a "Takeoff" command if requested
if (position[2] < 10.0) { // Check if altitude is below the desired takeoff height (e.g., 10m)
send_mavlink_command(S, MAV_CMD_NAV_TAKEOFF, 0, 10.0, 0); // Takeoff to 10 meters altitude
}

// Send a "Land" command if requested
if (position[2] > 10.0) { // Check if altitude is above the desired landing height (e.g., 10m)
send_mavlink_command(S, MAV_CMD_NAV_LAND, 0, 0, 0); // Land at current location
}

// Send PWM control data to ArduPilot for four motors (adjust as needed)
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];

uint16_t len = mavlink_msg_scaled_imu_pack(1, 200, &msg,
static_cast<uint32_t>(*timestamp),
static_cast<int16_t>(accel[0] * 1000), static_cast<int16_t>(accel[1] * 1000), static_cast<int16_t>(accel[2] * 1000),
static_cast<int16_t>(gyro[0] * 1000), static_cast<int16_t>(gyro[1] * 1000), static_cast<int16_t>(gyro[2] * 1000),
static_cast<int16_t>(mag[0] * 1000), static_cast<int16_t>(mag[1] * 1000), static_cast<int16_t>(mag[2] * 1000),
0); // Default temperature value of 0

len = mavlink_msg_to_send_buffer(buffer, &msg);
connector.send_message(buffer, len);

// Receive PWM signals for all four motors from ArduPilot
int pwm_signal[4] = {0, 0, 0, 0}; // Array to hold PWM signals for four motors
int ret = recvfrom(connector.sockfd, (char *)pwm_signal, sizeof(pwm_signal), 0, NULL, NULL);

if (ret > 0) {
real_T *output_pwm1 = ssGetOutputPortRealSignal(S, 0);
real_T *output_pwm2 = ssGetOutputPortRealSignal(S, 1);
real_T *output_pwm3 = ssGetOutputPortRealSignal(S, 2);
real_T *output_pwm4 = ssGetOutputPortRealSignal(S, 3);

// Map PWM signals for each motor to angular velocities
double min_pwm = 1000.0;
double max_pwm = 2000.0;
double min_angular_velocity = 0.0; // Minimum angular velocity (0 rad/s)
double max_angular_velocity = 10.0; // Maximum angular velocity (10 rad/s)

// Map each PWM signal to an angular velocity
*output_pwm1 = min_angular_velocity + (pwm_signal[0] – min_pwm) * (max_angular_velocity – min_angular_velocity) / (max_pwm – min_pwm);
*output_pwm2 = min_angular_velocity + (pwm_signal[1] – min_pwm) * (max_angular_velocity – min_angular_velocity) / (max_pwm – min_pwm);
*output_pwm3 = min_angular_velocity + (pwm_signal[2] – min_pwm) * (max_angular_velocity – min_angular_velocity) / (max_pwm – min_pwm);
*output_pwm4 = min_angular_velocity + (pwm_signal[3] – min_pwm) * (max_angular_velocity – min_angular_velocity) / (max_pwm – min_pwm);
}
}

void mdlTerminate(SimStruct *S) {
connector.~UDPConnector();
}

#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endifI have imported a 3D CAD model of a quadcopter into Simulink Multibody and am working to connect the Simulink model to ArduPilot using SITL. To achieve this, I am using an S-Function block that is designed to send input data from Simulink to ArduPilot and receive output data from ArduPilot back into Simulink, forming a closed-loop system.