diff --git a/aerpaw/basic_demo/handcode/comms.cpp b/aerpaw/basic_demo/comms.cpp similarity index 98% rename from aerpaw/basic_demo/handcode/comms.cpp rename to aerpaw/basic_demo/comms.cpp index 9579959..fb71d12 100644 --- a/aerpaw/basic_demo/handcode/comms.cpp +++ b/aerpaw/basic_demo/comms.cpp @@ -9,6 +9,8 @@ #define SERVER_PORT 5000 #define SERVER_IP "127.0.0.1" +#define OPCODE_SHUTDOWN 0xFF + // Socket init void initSockets() {} void cleanupSockets() {} diff --git a/aerpaw/basic_demo/compile.sh b/aerpaw/basic_demo/compile.sh index f0a5adc..874826f 100755 --- a/aerpaw/basic_demo/compile.sh +++ b/aerpaw/basic_demo/compile.sh @@ -1,7 +1,9 @@ #!/bin/bash #g++ -I/home/kdee/matlab/R2025a/extern/include controller_main.cpp controller.cpp controller_impl.cpp -o controller_app -lpthread -wd=$(pwd) -cd /home/kdee/Desktop/miSim/aerpaw/codegen -g++ -I/home/kdee/matlab/R2025a/extern/include -I../basic_demo/handcode -I. ../basic_demo/handcode/controller_main.cpp controller.cpp ../basic_demo/handcode/controller_impl.cpp controller_initialize.cpp controller_terminate.cpp -o controller_app -lpthread -cd $wd +#wd=$(pwd) +#cd /home/kdee/Desktop/miSim/aerpaw/codegen +g++ -I/home/kdee/matlab/R2025a/extern/include -I. controller_main.cpp controller.cpp controller_impl.cpp controller_initialize.cpp controller_terminate.cpp -o controller_app -lpthread +#cd $wd + +#g++ controller_main.cpp controller.cpp controller_impl.cpp controller_initialize.cpp controller_terminate.cpp diff --git a/aerpaw/basic_demo/controller.cpp b/aerpaw/basic_demo/controller.cpp index 32f5aef..f68385a 100644 --- a/aerpaw/basic_demo/controller.cpp +++ b/aerpaw/basic_demo/controller.cpp @@ -15,21 +15,48 @@ // Function Definitions void controller(int numClients) { + static const char b_filename[12]{"targets.txt"}; + double targets[12]; + char filename[12]; + // Maximum clients supported + // Allocate targets array (MAX_CLIENTS x 3) + // Load targets from file + // Define filename as null-terminated character array for C compatibility + for (int i{0}; i < 12; i++) { + targets[i] = 0.0; + filename[i] = b_filename[i]; + } + // loadTargets fills targets array (row-major: x1,y1,z1,x2,y2,z2,...) + loadTargets(&filename[0], &targets[0], 4); // Initialize server initServer(); // Accept clients for (int i{0}; i < numClients; i++) { acceptClient(i + 1); } - // Send messages to clients + // Send target coordinates to each client for (int i{0}; i < numClients; i++) { - sendMessage(i + 1); + double target[3]; + // Get target for this client (1x3 array) + target[0] = targets[i]; + target[1] = targets[i + 4]; + target[2] = targets[i + 8]; + sendTarget(i + 1, &target[0]); } - // Receive acknowledgements + // Receive TARGET acknowledgments for (int i{0}; i < numClients; i++) { - receiveAck(i + 1); + receiveTargetAck(i + 1); + } + // Check all ACKs received + // Wait for READY signals (UAVs have reached their targets) + for (int i{0}; i < numClients; i++) { + waitForReady(i + 1); + } + // Check all READY signals received + // Send COMPLETE to all clients before closing + for (int i{0}; i < numClients; i++) { + sendFinished(i + 1); } - // Digest ACKs // Close server closeServer(); } diff --git a/aerpaw/basic_demo/controller_app b/aerpaw/basic_demo/controller_app deleted file mode 100755 index e917ebb..0000000 Binary files a/aerpaw/basic_demo/controller_app and /dev/null differ diff --git a/aerpaw/basic_demo/controller_data.h b/aerpaw/basic_demo/controller_data.h new file mode 100644 index 0000000..a7a15dc --- /dev/null +++ b/aerpaw/basic_demo/controller_data.h @@ -0,0 +1,20 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_data.h +// +// Code generation for function 'controller_data' +// + +#ifndef CONTROLLER_DATA_H +#define CONTROLLER_DATA_H + +// Include files +#include "rtwtypes.h" +#include +#include + +#endif +// End of code generation (controller_data.h) diff --git a/aerpaw/basic_demo/controller_impl.cpp b/aerpaw/basic_demo/controller_impl.cpp new file mode 100644 index 0000000..c686efa --- /dev/null +++ b/aerpaw/basic_demo/controller_impl.cpp @@ -0,0 +1,160 @@ +#include "controller_impl.h" +#include +#include +#include +#include +#include +#include +#include + +#define SERVER_PORT 5000 +#define SERVER_IP "127.0.0.1" + +static int serverSocket = -1; +static std::vector clientSockets; + +void initSockets() {} +void cleanupSockets() {} + +void initServer() { + initSockets(); + serverSocket = socket(AF_INET, SOCK_STREAM, 0); + if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; } + + sockaddr_in serverAddr; + serverAddr.sin_family = AF_INET; + serverAddr.sin_addr.s_addr = INADDR_ANY; + serverAddr.sin_port = htons(SERVER_PORT); + + int opt = 1; + setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt)); + + if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { + std::cerr << "Bind failed\n"; return; + } + if(listen(serverSocket, 5) < 0) { + std::cerr << "Listen failed\n"; return; + } + + std::cout << "Server initialized\n"; +} + +void acceptClient(int clientId) { + sockaddr_in clientAddr; + socklen_t addrLen = sizeof(clientAddr); + int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen); + if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; } + clientSockets.push_back(clientSock); + std::cout << "Client " << clientId << " connected\n"; +} + +void sendMessage(int clientId) { + if(clientId <= 0 || clientId > clientSockets.size()) return; + const char* msg = "Hello from server"; + send(clientSockets[clientId-1], msg, strlen(msg), 0); + std::cout << "Sent message to client " << clientId << "\n"; +} + +int receiveAck(int clientId) { + if(clientId <= 0 || clientId > clientSockets.size()) return 0; + char buffer[1024]; + int len = recv(clientSockets[clientId-1], buffer, sizeof(buffer)-1, 0); + if(len <= 0) return 0; + buffer[len] = '\0'; + std::cout << "Received ACK from client " << clientId << ": " << buffer << "\n"; + return 1; +} + +void closeServer() { + for(auto sock : clientSockets) { + close(sock); + } + close(serverSocket); + cleanupSockets(); +} + +// Load target coordinates from file +// File format: one line per UAV with "x,y,z" coordinates +// Returns number of targets loaded +int loadTargets(const char* filename, double* targets, int maxClients) { + FILE* file = fopen(filename, "r"); + if (!file) { + std::cerr << "Failed to open targets file: " << filename << "\n"; + return 0; + } + + int count = 0; + double x, y, z; + // MATLAB uses column-major order, so for a maxClients x 3 matrix: + // Column 1 (x): indices 0, 1, 2, ... + // Column 2 (y): indices maxClients, maxClients+1, ... + // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ... + while (count < maxClients && fscanf(file, "%lf,%lf,%lf", &x, &y, &z) == 3) { + targets[count + 0 * maxClients] = x; // Column 1 + targets[count + 1 * maxClients] = y; // Column 2 + targets[count + 2 * maxClients] = z; // Column 3 + std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n"; + count++; + } + + fclose(file); + return count; +} + +// Send target coordinates to a client +// target points to 3 doubles: [x, y, z] +void sendTarget(int clientId, const double* target) { + if (clientId <= 0 || clientId > (int)clientSockets.size()) return; + + char buffer[256]; + snprintf(buffer, sizeof(buffer), "TARGET:%.6f,%.6f,%.6f", + target[0], target[1], target[2]); + + send(clientSockets[clientId - 1], buffer, strlen(buffer), 0); + std::cout << "Sent target to client " << clientId << ": " << buffer << "\n"; +} + +// Receive and validate ACK:TARGET response +// Returns 1 if ACK:TARGET received, 0 otherwise +int receiveTargetAck(int clientId) { + if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; + + char buffer[256]; + int len = recv(clientSockets[clientId - 1], buffer, sizeof(buffer) - 1, 0); + if (len <= 0) return 0; + buffer[len] = '\0'; + + std::cout << "Received from client " << clientId << ": " << buffer << "\n"; + + if (strncmp(buffer, "ACK:TARGET", 10) == 0) { + return 1; + } + return 0; +} + +// Wait for READY signal from client +// Returns 1 if READY received, 0 otherwise +int waitForReady(int clientId) { + if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; + + char buffer[256]; + int len = recv(clientSockets[clientId - 1], buffer, sizeof(buffer) - 1, 0); + if (len <= 0) return 0; + buffer[len] = '\0'; + + std::cout << "Received from client " << clientId << ": " << buffer << "\n"; + + if (strncmp(buffer, "READY", 5) == 0) { + return 1; + } + return 0; +} + +// Send COMPLETE message to signal graceful shutdown +void sendFinished(int clientId) { + if (clientId <= 0 || clientId > (int)clientSockets.size()) return; + + const char* msg = "FINISHED"; + send(clientSockets[clientId - 1], msg, strlen(msg), 0); + std::cout << "Sent FINISHED to client " << clientId << "\n"; +} diff --git a/aerpaw/basic_demo/handcode/controller_impl.h b/aerpaw/basic_demo/controller_impl.h similarity index 51% rename from aerpaw/basic_demo/handcode/controller_impl.h rename to aerpaw/basic_demo/controller_impl.h index 0e1328a..4781321 100644 --- a/aerpaw/basic_demo/handcode/controller_impl.h +++ b/aerpaw/basic_demo/controller_impl.h @@ -11,6 +11,13 @@ void sendMessage(int clientId); int receiveAck(int clientId); void closeServer(); +// Target location protocol functions +int loadTargets(const char* filename, double* targets, int maxClients); +void sendTarget(int clientId, const double* target); +int receiveTargetAck(int clientId); +int waitForReady(int clientId); +void sendFinished(int clientId); + #ifdef __cplusplus } #endif diff --git a/aerpaw/basic_demo/controller_initialize.cpp b/aerpaw/basic_demo/controller_initialize.cpp new file mode 100644 index 0000000..2510b0d --- /dev/null +++ b/aerpaw/basic_demo/controller_initialize.cpp @@ -0,0 +1,19 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_initialize.cpp +// +// Code generation for function 'controller_initialize' +// + +// Include files +#include "controller_initialize.h" + +// Function Definitions +void controller_initialize() +{ +} + +// End of code generation (controller_initialize.cpp) diff --git a/aerpaw/basic_demo/controller_initialize.h b/aerpaw/basic_demo/controller_initialize.h new file mode 100644 index 0000000..82e0ed2 --- /dev/null +++ b/aerpaw/basic_demo/controller_initialize.h @@ -0,0 +1,23 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_initialize.h +// +// Code generation for function 'controller_initialize' +// + +#ifndef CONTROLLER_INITIALIZE_H +#define CONTROLLER_INITIALIZE_H + +// Include files +#include "rtwtypes.h" +#include +#include + +// Function Declarations +extern void controller_initialize(); + +#endif +// End of code generation (controller_initialize.h) diff --git a/aerpaw/basic_demo/handcode/controller_main.cpp b/aerpaw/basic_demo/controller_main.cpp similarity index 99% rename from aerpaw/basic_demo/handcode/controller_main.cpp rename to aerpaw/basic_demo/controller_main.cpp index 09fc0ea..8a8120e 100644 --- a/aerpaw/basic_demo/handcode/controller_main.cpp +++ b/aerpaw/basic_demo/controller_main.cpp @@ -13,4 +13,4 @@ int main() { std::cout << "Server finished.\n"; return 0; -} +} \ No newline at end of file diff --git a/aerpaw/basic_demo/controller_terminate.cpp b/aerpaw/basic_demo/controller_terminate.cpp new file mode 100644 index 0000000..1577159 --- /dev/null +++ b/aerpaw/basic_demo/controller_terminate.cpp @@ -0,0 +1,19 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_terminate.cpp +// +// Code generation for function 'controller_terminate' +// + +// Include files +#include "controller_terminate.h" + +// Function Definitions +void controller_terminate() +{ +} + +// End of code generation (controller_terminate.cpp) diff --git a/aerpaw/basic_demo/controller_terminate.h b/aerpaw/basic_demo/controller_terminate.h new file mode 100644 index 0000000..723736f --- /dev/null +++ b/aerpaw/basic_demo/controller_terminate.h @@ -0,0 +1,23 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_terminate.h +// +// Code generation for function 'controller_terminate' +// + +#ifndef CONTROLLER_TERMINATE_H +#define CONTROLLER_TERMINATE_H + +// Include files +#include "rtwtypes.h" +#include +#include + +// Function Declarations +extern void controller_terminate(); + +#endif +// End of code generation (controller_terminate.h) diff --git a/aerpaw/basic_demo/controller_types.h b/aerpaw/basic_demo/controller_types.h new file mode 100644 index 0000000..33c4323 --- /dev/null +++ b/aerpaw/basic_demo/controller_types.h @@ -0,0 +1,18 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_types.h +// +// Code generation for function 'controller' +// + +#ifndef CONTROLLER_TYPES_H +#define CONTROLLER_TYPES_H + +// Include files +#include "rtwtypes.h" + +#endif +// End of code generation (controller_types.h) diff --git a/aerpaw/basic_demo/copy_codegen.sh b/aerpaw/basic_demo/copy_codegen.sh new file mode 100755 index 0000000..41d6a92 --- /dev/null +++ b/aerpaw/basic_demo/copy_codegen.sh @@ -0,0 +1,3 @@ +#!/bin/bash +cp ../codegen/*.h . +cp ../codegen/*.cpp . diff --git a/aerpaw/basic_demo/handcode/controller_impl.cpp b/aerpaw/basic_demo/handcode/controller_impl.cpp deleted file mode 100644 index dcfa649..0000000 --- a/aerpaw/basic_demo/handcode/controller_impl.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "controller_impl.h" -#include -#include -#include -#include -#include -#include - -#define SERVER_PORT 5000 -#define SERVER_IP "127.0.0.1" - -static int serverSocket = -1; -static std::vector clientSockets; - -void initSockets() {} -void cleanupSockets() {} - -void initServer() { - initSockets(); - serverSocket = socket(AF_INET, SOCK_STREAM, 0); - if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; } - - sockaddr_in serverAddr; - serverAddr.sin_family = AF_INET; - serverAddr.sin_addr.s_addr = INADDR_ANY; - serverAddr.sin_port = htons(SERVER_PORT); - - int opt = 1; - setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt)); - - if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { - std::cerr << "Bind failed\n"; return; - } - if(listen(serverSocket, 5) < 0) { - std::cerr << "Listen failed\n"; return; - } - - std::cout << "Server initialized\n"; -} - -void acceptClient(int clientId) { - sockaddr_in clientAddr; - socklen_t addrLen = sizeof(clientAddr); - int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen); - if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; } - clientSockets.push_back(clientSock); - std::cout << "Client " << clientId << " connected\n"; -} - -void sendMessage(int clientId) { - if(clientId <= 0 || clientId > clientSockets.size()) return; - const char* msg = "Hello from server"; - send(clientSockets[clientId-1], msg, strlen(msg), 0); - std::cout << "Sent message to client " << clientId << "\n"; -} - -int receiveAck(int clientId) { - if(clientId <= 0 || clientId > clientSockets.size()) return 0; - char buffer[1024]; - int len = recv(clientSockets[clientId-1], buffer, sizeof(buffer)-1, 0); - if(len <= 0) return 0; - buffer[len] = '\0'; - std::cout << "Received ACK from client " << clientId << ": " << buffer << "\n"; - return 1; -} - -void closeServer() { - for(auto sock : clientSockets) { - close(sock); - } - close(serverSocket); - cleanupSockets(); -} diff --git a/aerpaw/basic_demo/targets.txt b/aerpaw/basic_demo/targets.txt new file mode 100644 index 0000000..3165440 --- /dev/null +++ b/aerpaw/basic_demo/targets.txt @@ -0,0 +1,2 @@ +10.5,20.3,45.0 +25.0,30.0,30.0 \ No newline at end of file diff --git a/aerpaw/basic_demo/tmwtypes.h b/aerpaw/basic_demo/tmwtypes.h new file mode 100644 index 0000000..0ee3d69 --- /dev/null +++ b/aerpaw/basic_demo/tmwtypes.h @@ -0,0 +1,888 @@ +/* + * Copyright 1984-2023 The MathWorks, Inc. + */ + +#if defined(_MSC_VER) +# pragma once +#endif +#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3)) +# pragma once +#endif + +#ifndef tmwtypes_h +#define tmwtypes_h + +#ifndef __TMWTYPES__ +#define __TMWTYPES__ +/* + * File : tmwtypes.h + * Abstract: + * Data types for use with MATLAB/SIMULINK and the Real-Time Workshop. + * + * When compiling stand-alone model code, data types can be overridden + * via compiler switches. + * + * Define NO_FLOATS to eliminate reference to real_T, etc. + */ + +#ifdef MW_LIBTOOLING +#include "mwstdint.h" +#endif + +#include + +/* __STDC_VERSION__ version check below means "check for a C99 compiler". + + Visual Studio (checked on versions 2015 and 2017) does + not define __STDC_VERSION__, however it has stdbool.h available, + thus a separate check for _MSC_VER below. + */ +#if defined(__APPLE_CC__) \ + || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \ + || (defined(_MSC_VER) && (_MSC_VER >= 1900)) +#ifndef tmwtypes_do_not_include_stdbool +#include +#endif +#endif + +#define LOGICAL_IS_A_TYPE +#define SPARSE_GENERALIZATION + +#ifdef NO_FLOATS +# define double double_not_allowed +# define float float_not_allowed +#endif /*NO_FLOATS*/ + +#ifndef NO_FLOATS + +#ifndef __MWERKS__ +# ifdef __STDC__ +# include +# else +# ifndef FLT_MANT_DIG +# define FLT_MANT_DIG 24 +# endif +# ifndef DBL_MANT_DIG +# define DBL_MANT_DIG 53 +# endif +# endif +#endif + +#endif /*NO_FLOATS*/ + +/* + * The following data types cannot be overridden when building MEX files. + */ +#ifdef MATLAB_MEX_FILE +# undef CHARACTER_T +# undef INTEGER_T +# undef BOOLEAN_T +# undef REAL_T +# undef TIME_T +#endif + +/* + * The uchar_T, ushort_T and ulong_T types are needed for compilers which do + * not allow defines to be specified, at the command line, with spaces in them. + */ + +typedef unsigned char uchar_T; +typedef unsigned short ushort_T; +typedef unsigned long ulong_T; + +#if (defined(_MSC_VER) && _MSC_VER >= 1500) \ + || defined(__x86_64__) || defined(__LP64__) \ + || defined(__LCC64__) + +typedef unsigned long long ulonglong_T; +#endif + + + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +/* When used with Real Time Workshop generated code, this + * header file can be used with a variety of compilers. + * + * The compiler could be for an 8 bit embedded processor that + * only had 8 bits per integer and 16 bits per long. + * In that example, a 32 bit integer size is not even available. + * This header file should be robust to that. + * + * For the case of an 8 bit processor, the preprocessor + * may be limited to 16 bit math like its target. That limitation + * would mean that 32 bit comparisons can't be done accurately. + * To increase robustness to this, comparisons are done against + * smaller values first. An inaccurate 32 bit comparison isn't + * attempted if the 16 bit comparison has already succeeded. + * + * Limitations on preprocessor math can also be stricter than + * for the target. There are known cases where a compiler + * targeting processors with 64 bit longs can't do accurate + * preprocessor comparisons on more than 32 bits. + */ + +/* Determine the number of bits for int, long, short, and char. + * If one fails to be determined, set the number of bits to -1 + */ + +#ifndef TMW_BITS_PER_INT +# if INT_MAX == 0x7FL +# define TMW_BITS_PER_INT 8 +# elif INT_MAX == 0x7FFFL +# define TMW_BITS_PER_INT 16 +# elif INT_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_INT 32 +# else +# define TMW_BITS_PER_INT -1 +# endif +#endif + +#ifndef TMW_BITS_PER_LONG +# if LONG_MAX == 0x7FL +# define TMW_BITS_PER_LONG 8 +# elif LONG_MAX == 0x7FFFL +# define TMW_BITS_PER_LONG 16 +# elif LONG_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_LONG 32 +# else +# define TMW_BITS_PER_LONG -1 +# endif +#endif + +#ifndef TMW_BITS_PER_SHRT +# if SHRT_MAX == 0x7FL +# define TMW_BITS_PER_SHRT 8 +# elif SHRT_MAX == 0x7FFFL +# define TMW_BITS_PER_SHRT 16 +# elif SHRT_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_SHRT 32 +# else +# define TMW_BITS_PER_SHRT -1 +# endif +#endif + +#ifndef TMW_BITS_PER_SCHAR +# if SCHAR_MAX == 0x7FL +# define TMW_BITS_PER_SCHAR 8 +# elif SCHAR_MAX == 0x7FFFL +# define TMW_BITS_PER_SCHAR 16 +# elif SCHAR_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_SCHAR 32 +# else +# define TMW_BITS_PER_SCHAR -1 +# endif +#endif + +#ifndef TMW_CHAR_SIGNED +# if SCHAR_MAX == CHAR_MAX +# define TMW_CHAR_SIGNED 1 +# else +# define TMW_CHAR_SIGNED 0 +# endif +#endif + +/* It is common for one or more of the integer types + * to be the same size. For example, on many embedded + * processors, both shorts and ints are 16 bits. On + * processors used for workstations, it is quite common + * for both int and long to be 32 bits. + * When there is more than one choice for typdef'ing + * a portable type like int16_T or uint32_T, in + * concept, it should not matter which choice is made. + * However, some style guides and some code checking + * tools do identify and complain about seemingly + * irrelevant differences. For example, a code + * checking tool may complain about an implicit + * conversion from int to short even though both + * are 16 bits. To reduce these types of + * complaints, it is best to make int the + * preferred choice when more than one is available. + */ + +#ifndef INT8_T +# if defined(MW_LIBTOOLING) +# define INT8_T int8_t +# elif TMW_BITS_PER_INT == 8 +# define INT8_T int +# elif TMW_BITS_PER_LONG == 8 +# define INT8_T long +# elif TMW_BITS_PER_SCHAR == 8 +# define INT8_T signed char +# elif TMW_BITS_PER_SHRT == 8 +# define INT8_T short +# endif +#endif +#ifdef INT8_T + typedef INT8_T int8_T; +#endif + +#ifndef UINT8_T +# if defined(MW_LIBTOOLING) +# define UINT8_T uint8_t +# elif TMW_BITS_PER_INT == 8 +# define UINT8_T unsigned int +# elif TMW_BITS_PER_LONG == 8 +# define UINT8_T unsigned long +# elif TMW_BITS_PER_SCHAR == 8 +# define UINT8_T unsigned char +# elif TMW_BITS_PER_SHRT == 8 +# define UINT8_T unsigned short +# endif +#endif +#ifdef UINT8_T + typedef UINT8_T uint8_T; +#endif + + +#ifndef INT16_T +# if defined(MW_LIBTOOLING) +# define INT16_T int16_t +# elif TMW_BITS_PER_INT == 16 +# define INT16_T int +# elif TMW_BITS_PER_LONG == 16 +# define INT16_T long +# elif TMW_BITS_PER_SCHAR == 16 +# define INT16_T signed char +# elif TMW_BITS_PER_SHRT == 16 +# define INT16_T short +# endif +#endif +#ifdef INT16_T + typedef INT16_T int16_T; +#endif + + +#ifndef UINT16_T +# if defined(MW_LIBTOOLING) +# define UINT16_T uint16_t +# elif TMW_BITS_PER_INT == 16 +# define UINT16_T unsigned int +# elif TMW_BITS_PER_LONG == 16 +# define UINT16_T unsigned long +# elif TMW_BITS_PER_SCHAR == 16 +# define UINT16_T unsigned char +# elif TMW_BITS_PER_SHRT == 16 +# define UINT16_T unsigned short +# endif +#endif +#ifdef UINT16_T + typedef UINT16_T uint16_T; +#endif + + +#ifndef INT32_T +# if defined(MW_LIBTOOLING) +# define INT32_T int32_t +# elif TMW_BITS_PER_INT == 32 +# define INT32_T int +# elif TMW_BITS_PER_LONG == 32 +# define INT32_T long +# elif TMW_BITS_PER_SCHAR == 32 +# define INT32_T signed char +# elif TMW_BITS_PER_SHRT == 32 +# define INT32_T short +# endif +#endif +#ifdef INT32_T + typedef INT32_T int32_T; +#endif + + +#ifndef UINT32_T +# if defined(MW_LIBTOOLING) +# define UINT32_T uint32_t +# elif TMW_BITS_PER_INT == 32 +# define UINT32_T unsigned int +# elif TMW_BITS_PER_LONG == 32 +# define UINT32_T unsigned long +# elif TMW_BITS_PER_SCHAR == 32 +# define UINT32_T unsigned char +# elif TMW_BITS_PER_SHRT == 32 +# define UINT32_T unsigned short +# endif +#endif +#ifdef UINT32_T + typedef UINT32_T uint32_T; +#endif + +/* The following is used to emulate smaller integer types when only + * larger types are available. For example, compilers for TI C3x/C4x DSPs + * define char and short to be 32 bits, so 8 and 16 bits are not directly + * available. This target is commonly used with RTW rapid prototyping. + * Other DSPs define char to be 16 bits, so 8 bits is not directly + * available. + */ +#ifndef INT8_T +# ifdef INT16_T +# define INT8_T INT16_T + typedef INT8_T int8_T; +# else +# ifdef INT32_T +# define INT8_T INT32_T + typedef INT8_T int8_T; +# endif +# endif +#endif + +#ifndef UINT8_T +# ifdef UINT16_T +# define UINT8_T UINT16_T + typedef UINT8_T uint8_T; +# else +# ifdef UINT32_T +# define UINT8_T UINT32_T + typedef UINT8_T uint8_T; +# endif +# endif +#endif + +#ifndef INT16_T +# ifdef INT32_T +# define INT16_T INT32_T + typedef INT16_T int16_T; +# endif +#endif + +#ifndef UINT16_T +# ifdef UINT32_T +# define UINT16_T UINT32_T + typedef UINT16_T uint16_T; +# endif +#endif + + +#ifndef NO_FLOATS + +#ifndef REAL32_T +# ifndef __MWERKS__ +# if FLT_MANT_DIG >= 23 +# define REAL32_T float +# endif +# else +# define REAL32_T float +# endif +#endif +#ifdef REAL32_T + typedef REAL32_T real32_T; +#endif + + +#ifndef REAL64_T +# ifndef __MWERKS__ +# if DBL_MANT_DIG >= 52 +# define REAL64_T double +# endif +# else +# define REAL64_T double +# endif +#endif +#ifdef REAL64_T + typedef REAL64_T real64_T; +#endif + +#endif /* NO_FLOATS*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int64_T - signed 64 bit integers * + * uint64_T - unsigned 64 bit integers * + *=======================================================================*/ + +# if defined(MW_LIBTOOLING) +# ifdef INT64_T +# undef INT64_T +# endif +# define INT64_T int64_t +# ifdef UINT64_T +# undef UINT64_T +# endif +# define UINT64_T uint64_t +# endif +#if !defined(INT64_T) || !defined(UINT64_T) || !defined(FMT64) +# if defined(__APPLE__) || defined(__clang__) +# ifndef INT64_T +# define INT64_T long long +# endif +# ifndef UINT64_T +# define UINT64_T unsigned long long +# endif +# ifndef FMT64 +# define FMT64 "ll" +# endif +# if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG) +# define INT_TYPE_64_IS_LONG +# endif +# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__) +# ifndef INT64_T +# define INT64_T long +# endif +# ifndef UINT64_T +# define UINT64_T unsigned long +# endif +# ifndef FMT64 +# define FMT64 "l" +# endif +# if !defined(INT_TYPE_64_IS_LONG) +# define INT_TYPE_64_IS_LONG +# endif +# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) +# ifndef INT64_T +# define INT64_T __int64 +# endif +# ifndef UINT64_T +# define UINT64_T unsigned __int64 +# endif +# ifndef FMT64 +# define FMT64 "I64" +# endif +# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \ + || defined(__LCC64__) +# ifndef INT64_T +# define INT64_T long long +# endif +# ifndef UINT64_T +# define UINT64_T unsigned long long +# endif +# ifndef FMT64 +# define FMT64 "ll" +# endif +# endif + +#endif + +#if defined(INT64_T) +# if defined(__GNUC__) && \ + ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9))) + __extension__ +# endif + typedef INT64_T int64_T; +#endif + +#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \ + || defined(__x86_64__) \ + || defined(__LP64__) +# define INT_TYPE_64_IS_SUPPORTED +#endif + +#if defined(UINT64_T) +# if defined(__GNUC__) && \ + ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9))) + __extension__ +# endif + typedef UINT64_T uint64_T; +#endif + +/*===========================================================================* + * Format string modifiers for using size_t variables in printf statements. * + *===========================================================================*/ + +#ifndef FMT_SIZE_T +# if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__) +# define FMT_SIZE_T "z" +# elif defined (__WATCOMC__) +# define FMT_SIZE_T "l" +# elif defined (_WIN32 ) +# define FMT_SIZE_T "I" +# else +# define FMT_SIZE_T "l" +# endif +#endif + +#ifndef FMT_PTRDIFF_T +# if defined(__APPLE__) +# define FMT_PTRDIFF_T "l" +# elif defined( __GNUC__ ) || defined(_STDC_C99) +# define FMT_PTRDIFF_T "t" +# elif defined (__WATCOMC__) +# define FMT_PTRDIFF_T "l" +# elif defined (_WIN32 ) +# define FMT_PTRDIFF_T "I" +# else +# define FMT_PTRDIFF_T "l" +# endif +#endif + +/*===========================================================================* + * General or logical data types where the word size is not guaranteed. * + * real_T - possible settings include real32_T or real64_T * + * time_T - possible settings include real32_T or real64_T * + * boolean_T * + * char_T * + * int_T * + * uint_T * + * byte_T * + *===========================================================================*/ + +#ifndef NO_FLOATS + +#ifndef REAL_T +# ifdef REAL64_T +# define REAL_T real64_T +# else +# ifdef REAL32_T +# define REAL_T real32_T +# endif +# endif +#endif +#ifdef REAL_T + typedef REAL_T real_T; +#endif + +#ifndef TIME_T +# ifdef REAL_T +# define TIME_T real_T +# endif +#endif +#ifdef TIME_T + typedef TIME_T time_T; +#endif + +#endif /* NO_FLOATS */ + +#ifndef BOOLEAN_T +# if defined(UINT8_T) +# define BOOLEAN_T UINT8_T +# else +# define BOOLEAN_T unsigned int +# endif +#endif +typedef BOOLEAN_T boolean_T; + + +#ifndef CHARACTER_T +# define CHARACTER_T char +#endif +typedef CHARACTER_T char_T; + + +#ifndef INTEGER_T +# define INTEGER_T int +#endif +typedef INTEGER_T int_T; + + +#ifndef UINTEGER_T +# define UINTEGER_T unsigned +#endif +typedef UINTEGER_T uint_T; + + +#ifndef BYTE_T +# define BYTE_T unsigned char +#endif +typedef BYTE_T byte_T; + + +/*===========================================================================* + * Define Complex Structures * + *===========================================================================*/ +#ifndef NO_FLOATS + +#ifndef CREAL32_T +# ifdef REAL32_T + typedef struct { + real32_T re, im; + } creal32_T; +# define CREAL32_T creal32_T +# endif +#endif + +#ifndef CREAL64_T +# ifdef REAL64_T + typedef struct { + real64_T re, im; + } creal64_T; +# define CREAL64_T creal64_T +# endif +#endif + +#ifndef CREAL_T +# ifdef REAL_T + typedef struct { + real_T re, im; + } creal_T; +# define CREAL_T creal_T +# endif +#endif + +#endif /* NO_FLOATS */ + +#ifndef CINT8_T +# ifdef INT8_T + typedef struct { + int8_T re, im; + } cint8_T; +# define CINT8_T cint8_T +# endif +#endif + +#ifndef CUINT8_T +# ifdef UINT8_T + typedef struct { + uint8_T re, im; + } cuint8_T; +# define CUINT8_T cuint8_T +# endif +#endif + +#ifndef CINT16_T +# ifdef INT16_T + typedef struct { + int16_T re, im; + } cint16_T; +# define CINT16_T cint16_T +# endif +#endif + +#ifndef CUINT16_T +# ifdef UINT16_T + typedef struct { + uint16_T re, im; + } cuint16_T; +# define CUINT16_T cuint16_T +# endif +#endif + +#ifndef CINT32_T +# ifdef INT32_T + typedef struct { + int32_T re, im; + } cint32_T; +# define CINT32_T cint32_T +# endif +#endif + +#ifndef CUINT32_T +# ifdef UINT32_T + typedef struct { + uint32_T re, im; + } cuint32_T; +# define CUINT32_T cuint32_T +# endif +#endif + +#ifndef CINT64_T +# ifdef INT64_T + typedef struct { + int64_T re, im; + } cint64_T; +# define CINT64_T cint64_T +# endif +#endif + +#ifndef CUINT64_T +# ifdef UINT64_T + typedef struct { + uint64_T re, im; + } cuint64_T; +# define CUINT64_T cuint64_T +# endif +#endif + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) /* 127 */ +#define MIN_int8_T ((int8_T)(-128)) /* -128 */ +#define MAX_uint8_T ((uint8_T)(255)) /* 255 */ +#define MIN_uint8_T ((uint8_T)(0)) + +#define MAX_int16_T ((int16_T)(32767)) /* 32767 */ +#define MIN_int16_T ((int16_T)(-32768)) /* -32768 */ +#define MAX_uint16_T ((uint16_T)(65535)) /* 65535 */ +#define MIN_uint16_T ((uint16_T)(0)) + +#define MAX_int32_T ((int32_T)(2147483647)) /* 2147483647 */ +#define MIN_int32_T ((int32_T)(-2147483647-1)) /* -2147483648 */ +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) /* 4294967295 */ +#define MIN_uint32_T ((uint32_T)(0)) + +#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) \ + || defined(__LCC64__) +# ifdef INT64_T +# define MAX_int64_T ((int64_T)(9223372036854775807LL)) +# define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +# endif +# ifdef UINT64_T +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) +# define MIN_uint64_T ((uint64_T)(0)) +# endif +#else +# ifdef INT64_T +# ifdef INT_TYPE_64_IS_LONG +# define MAX_int64_T ((int64_T)(9223372036854775807L)) +# define MIN_int64_T ((int64_T)(-9223372036854775807L-1L)) +# else +# define MAX_int64_T ((int64_T)(9223372036854775807LL)) +# define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +# endif +# endif +# ifdef UINT64_T +# ifdef INT_TYPE_64_IS_LONG +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL)) +# define MIN_uint64_T ((uint64_T)(0)) +# else +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) +# define MIN_uint64_T ((uint64_T)(0)) +# endif +# endif +#endif + +#if (defined(_MSC_VER) && !defined(__clang__)) + +/* Conversion from unsigned __int64 to double is not implemented in Visual Studio + * and results in a compile error, thus the value must first be cast to + * signed __int64, and then to double. + * + * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max, + * the macro below provides a workaround for casting a uint64 value to a double + * in windows. + */ +# define uint64_to_double(u) ( ((u) > _I64_MAX) ? \ + (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \ + (double)(__int64)(u) ) + +/* The following inline function should only be used in the macro double_to_uint64, + * as it only handles the specfic range of double between 2^63 and 2^64-1 */ +__forceinline +uint64_T double_to_uint64_helper(double d) { + union double_to_uint64_union_type { + double dd; + uint64_T i64; + } di; + di.dd = d; + return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11); +} + +/* The largest double value that can be cast to uint64 in windows is the + * signed int64 max, which is 2^63-1. The macro below provides + * a workaround for casting large double values to uint64 in windows. + */ +/* The magic number 18446744073709551616.0 is 2^64 */ +/* The magic number 9223372036854775808.0 is 2^63 */ +# define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \ + 0xffffffffffffffffULL : \ + ((d) >= 0.0) ? \ + ((d) >= 9223372036854775808.0) ? \ + double_to_uint64_helper(d) : \ + (unsigned __int64)(d) : \ + 0ULL ) +#else +# define uint64_to_double(u) ((double)(u)) +# if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__) +/* double_to_uint64 defined only for MSVC and UNIX */ +# else +# define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \ + (unsigned long long) 0xffffffffffffffffULL : \ + ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 ) +# endif +#endif + +#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined) + +#ifndef _bool_T +#define _bool_T + +typedef boolean_T bool; + +#ifndef false +#define false (0) +#endif +#ifndef true +#define true (1) +#endif + +#endif /* _bool_T */ + +#endif /* !__cplusplus */ + +/* + * This software assumes that the code is being compiled on a target using a + * 2's complement representation for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable/model) + * including the null-termination character. + */ +#define TMW_NAME_LENGTH_MAX 2049 + +/* + * Maximum values for indices and dimensions + */ +#include + +#ifdef MX_COMPAT_32 +typedef int mwSize; +typedef int mwIndex; +typedef int mwSignedIndex; +#else +typedef size_t mwSize; /* unsigned pointer-width integer */ +typedef size_t mwIndex; /* unsigned pointer-width integer */ +typedef ptrdiff_t mwSignedIndex; /* a signed pointer-width integer */ +#endif + + /* for the individual dim */ +/* If updating SLSize or SLIndex, update defintions in sl_types_def.h + as well. */ +#ifndef SLSIZE_SLINDEX + #define SLSIZE_SLINDEX + #ifdef INT_TYPE_64_IS_SUPPORTED + typedef int64_T SLIndex; + typedef int64_T SLSize; + #else + typedef int SLIndex; + typedef int SLSize; + #endif +#endif + +/* for the total size */ +#define SLIndexType size_t +#define INVALID_SIZET_VALUE (std::numeric_limits::max()) +#define MAX_VALID_SIZET_VALUE (std::numeric_limits::max() -1) + + +#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32) +/* Currently 2^48 based on hardware limitations */ +# define MWSIZE_MAX 281474976710655UL +# define MWINDEX_MAX 281474976710655UL +# define MWSINDEX_MAX 281474976710655L +# define MWSINDEX_MIN -281474976710655L +#else +# define MWSIZE_MAX 2147483647UL +# define MWINDEX_MAX 2147483647UL +# define MWSINDEX_MAX 2147483647L +# define MWSINDEX_MIN -2147483647L +#endif +#define MWSIZE_MIN 0UL +#define MWINDEX_MIN 0UL + +/** UTF-16 character type */ + +#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT) +typedef char16_t CHAR16_T; +#define U16_STRING_LITERAL_PREFIX u +#elif defined(_MSC_VER) +typedef wchar_t CHAR16_T; +#define U16_STRING_LITERAL_PREFIX L +#else +typedef UINT16_T CHAR16_T; +#endif + +#endif /* __TMWTYPES__ */ + +#endif /* tmwtypes_h */ diff --git a/aerpaw/basic_demo/uav.py b/aerpaw/basic_demo/uav.py index d88f923..3dafa56 100755 --- a/aerpaw/basic_demo/uav.py +++ b/aerpaw/basic_demo/uav.py @@ -1,13 +1,97 @@ -#!/bin/python3 +#!/usr/bin/env python3 +""" +UAV Client for AERPAW Target Location Protocol + +Protocol: + Server sends: TARGET:x,y,z + Client sends: ACK:TARGET + Client (after moving): READY +""" + import socket +import sys +import time SERVER_IP = "127.0.0.1" SERVER_PORT = 5000 -for client_id in range(2): # match numClients + +def parse_target(message: str) -> tuple: + """Parse TARGET:x,y,z message and return (x, y, z) coordinates.""" + if not message.startswith("TARGET:"): + raise ValueError(f"Invalid message format: {message}") + + coords_str = message[7:] # Remove "TARGET:" prefix + parts = coords_str.split(",") + if len(parts) != 3: + raise ValueError(f"Expected 3 coordinates, got {len(parts)}") + + x, y, z = float(parts[0]), float(parts[1]), float(parts[2]) + return (x, y, z) + + +def move_to_target(x: float, y: float, z: float): + """ + Placeholder for AERPAW API integration. + In the future, this will command the UAV to move to (x, y, z). + """ + print(f" [PLACEHOLDER] Moving to target: ({x}, {y}, {z})") + # Simulate movement time + time.sleep(1.0) + print(f" [PLACEHOLDER] Arrived at target: ({x}, {y}, {z})") + + +def run_uav_client(client_id: int): + """Run a single UAV client.""" + print(f"UAV {client_id}: Connecting to {SERVER_IP}:{SERVER_PORT}") + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((SERVER_IP, SERVER_PORT)) - data = s.recv(1024) # receive message from server - print(f"Client {client_id+1} received: {data.decode()}") - s.sendall(b"ACK") # send acknowledgment - s.close() + + try: + # Receive TARGET command + data = s.recv(1024).decode().strip() + print(f"UAV {client_id}: Received: {data}") + + # Parse target coordinates + x, y, z = parse_target(data) + print(f"UAV {client_id}: Parsed target: x={x}, y={y}, z={z}") + + # Send acknowledgment + s.sendall(b"ACK:TARGET") + print(f"UAV {client_id}: Sent ACK:TARGET") + + # Move to target (placeholder) + move_to_target(x, y, z) + + # Signal ready + s.sendall(b"READY") + print(f"UAV {client_id}: Sent READY") + + # Wait for FINISHED signal from server + data = s.recv(1024).decode().strip() + if data == "FINISHED": + print(f"UAV {client_id}: Received FINISHED - session ended normally") + else: + print(f"UAV {client_id}: Unexpected message: {data}") + + except ValueError as e: + error_msg = f"ERROR:{str(e)}" + s.sendall(error_msg.encode()) + print(f"UAV {client_id}: Error - {e}") + + finally: + s.close() + print(f"UAV {client_id}: Connection closed") + + +if __name__ == "__main__": + if len(sys.argv) != 2: + print(f"Usage: {sys.argv[0]} ") + print(" Run one instance per UAV, e.g.:") + print(" Terminal 1: ./uav.py 1") + print(" Terminal 2: ./uav.py 2") + sys.exit(1) + + client_id = int(sys.argv[1]) + run_uav_client(client_id) diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index 5aa1d80..1a4fe3e 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -4,7 +4,7 @@ - + @@ -38,6 +38,11 @@ + + int32 + + + @@ -63,89 +68,71 @@ /home/kdee/Desktop/miSim/aerpaw/codegen - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/comms.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/controller_impl.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/controller_impl.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h + /home/kdee/matlab/R2025a/extern/include/tmwtypes.h GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h - - GENERATED_SOURCE - - - - /home/kdee/matlab/R2025a/extern/include/tmwtypes.h - - GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp GENERATED_SOURCE - + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h @@ -153,7 +140,7 @@ true - 2026-01-28T23:10:50 + 2026-01-29T16:33:06 @@ -176,36 +163,21 @@ - customInclude - - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode - - - - - customSource - - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/comms.cpp - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/controller_impl.cpp - - - - genCodeOnly - + true - + generateReport - + - + targetLang - + C++ diff --git a/aerpaw/controller.m b/aerpaw/controller.m index cfb67d5..3ff3615 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -3,17 +3,37 @@ arguments (Input) numClients (1, 1) int32; end -coder.extrinsic('disp'); +coder.extrinsic('disp', 'readmatrix'); + +% Maximum clients supported +MAX_CLIENTS = 4; + +% Allocate targets array (MAX_CLIENTS x 3) +targets = zeros(MAX_CLIENTS, 3); + +% Load targets from file +if coder.target('MATLAB') + disp('Loading targets from file (simulation)...'); + targetsLoaded = readmatrix('targets.txt'); + numTargets = min(size(targetsLoaded, 1), numClients); + targets(1:numTargets, :) = targetsLoaded(1:numTargets, :); + disp(['Loaded ', num2str(numTargets), ' targets']); +else + coder.cinclude('controller_impl.h'); + % Define filename as null-terminated character array for C compatibility + filename = ['targets.txt', char(0)]; + % loadTargets fills targets array (row-major: x1,y1,z1,x2,y2,z2,...) + coder.ceval('loadTargets', coder.ref(filename), ... + coder.ref(targets), int32(MAX_CLIENTS)); +end % Initialize server if coder.target('MATLAB') disp('Initializing server (simulation)...'); else - coder.cinclude('controller_impl.h'); coder.ceval('initServer'); end - % Accept clients for i = 1:numClients if coder.target('MATLAB') @@ -23,33 +43,60 @@ for i = 1:numClients end end - -% Send messages to clients +% Send target coordinates to each client for i = 1:numClients + % Get target for this client (1x3 array) + target = targets(i, :); + if coder.target('MATLAB') - disp(['Sending message to client ', num2str(i)]); + disp(['Sending TARGET to client ', num2str(i), ': ', ... + num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]); else - coder.ceval('sendMessage', int32(i)); + coder.ceval('sendTarget', int32(i), coder.ref(target)); end end -% Receive acknowledgements -acksReceived = zeros(1, numClients, 'int32'); +% Receive TARGET acknowledgments +targetAcks = zeros(1, numClients, 'int32'); for i = 1:numClients if coder.target('MATLAB') - disp(['Receiving ACK from client ', num2str(i)]); - acksReceived(i) = 1; % Simulate successful ACK + disp(['Waiting for ACK:TARGET from client ', num2str(i)]); + targetAcks(i) = 1; % Simulate successful ACK else - acksReceived(i) = coder.ceval('receiveAck', int32(i)); + targetAcks(i) = coder.ceval('receiveTargetAck', int32(i)); end end - -% Digest ACKs +% Check all ACKs received if coder.target('MATLAB') - disp(['All ACKs received: ', num2str(acksReceived)]); + disp(['Target ACKs received: ', num2str(targetAcks)]); end +% Wait for READY signals (UAVs have reached their targets) +readySignals = zeros(1, numClients, 'int32'); +for i = 1:numClients + if coder.target('MATLAB') + disp(['Waiting for READY from client ', num2str(i)]); + readySignals(i) = 1; % Simulate READY + else + readySignals(i) = coder.ceval('waitForReady', int32(i)); + end +end + +% Check all READY signals received +if coder.target('MATLAB') + disp(['Ready signals received: ', num2str(readySignals)]); + disp('All UAVs at target positions.'); +end + +% Send COMPLETE to all clients before closing +for i = 1:numClients + if coder.target('MATLAB') + disp(['Sending COMPLETE to client ', num2str(i)]); + else + coder.ceval('sendFinished', int32(i)); + end +end % Close server if ~coder.target('MATLAB')