From 6fb6849a5da8408fee7c804e33b82d50737a7a1b Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 8 Jan 2025 07:10:50 +0300 Subject: [PATCH] basic ArduPlane JSON integration --- .../ardupilot_json_interface.hpp | 216 +++++++++++++----- src/Simulator/src/main.cpp | 29 ++- 2 files changed, 192 insertions(+), 53 deletions(-) diff --git a/src/ArdupilotInterface/include/ArdupilotInterface/ardupilot_json_interface.hpp b/src/ArdupilotInterface/include/ArdupilotInterface/ardupilot_json_interface.hpp index bbbb81b..53f2380 100644 --- a/src/ArdupilotInterface/include/ArdupilotInterface/ardupilot_json_interface.hpp +++ b/src/ArdupilotInterface/include/ArdupilotInterface/ardupilot_json_interface.hpp @@ -1,6 +1,3 @@ -#ifndef ARDUPILOT_JSON_INTERFACE_ARDUPILOT_JSON_INTERFACE_HPP -#define ARDUPILOT_JSON_INTERFACE_ARDUPILOT_JSON_INTERFACE_HPP - /* * This file is free software: you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the @@ -13,75 +10,190 @@ * See the GNU General Public License for more details. * * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - * + * with this program. If not, see . */ -// This is very minimal example of using libAP_JSON to send simulator data to ArduPilot SITL +#ifndef ARDUPILOT_JSON_INTERFACE_ARDUPILOT_JSON_INTERFACE_HPP +#define ARDUPILOT_JSON_INTERFACE_ARDUPILOT_JSON_INTERFACE_HPP -#include #include -#include - +#include +#include +#include +#include +#include +#include #include "libAP_JSON.cpp" +class ArdupilotJsonInterface +{ +public: + ArdupilotJsonInterface() : _inited(false), _ap_online(false) {} -static inline uint64_t micros() { - uint64_t us = std::chrono::duration_cast(std::chrono::high_resolution_clock:: - now().time_since_epoch()).count(); - return us; -} - -static inline int ardupilot_json_interface() { - uint16_t servo_out[16]; + int Init(const std::string &ip, int port) + { + if (_libAP.InitSockets(ip.c_str(), port)) { + std::cout << "[ArdupilotJsonInterface] Connected to ArduPilot SITL at " + << ip << ":" << port << std::endl; + _inited = true; + return 0; + } + std::cerr << "[ArdupilotJsonInterface] ERROR: Failed to init sockets." << std::endl; + return -1; + } - // init the ArduPilot connection - libAP_JSON ap; - if (ap.InitSockets("127.0.0.1", 9002)) + int Clean() { - std::cout << "started socket" << std::endl; + _inited = false; + return 0; } - // send and receive data from AP - while (true) + int Receive(bool /*blocking*/, bool &armed, std::array &command) { - double timestamp = (double) micros() / 1000000.0; - - if (ap.ReceiveServoPacket(servo_out)) - { -// #if DEBUG_ENABLED - std::cout << "servo_out PWM: ["; - for (int i = 0; i < MAX_SERVO_CHANNELS - 1; ++i) - { - std::cout << servo_out[i] << ", "; + if (!_inited) { + std::cerr << "[ArdupilotJsonInterface] ERROR: Not initialized." << std::endl; + return -1; + } + uint16_t servo_out[MAX_SERVO_CHANNELS]; + bool got_packet = _libAP.ReceiveServoPacket(servo_out); + if (got_packet) { + bool all_zero = true; + for (size_t i = 0; i < MAX_SERVO_CHANNELS; i++) { + if (servo_out[i] != 0) { + all_zero = false; + break; + } + } + armed = !all_zero; + for (size_t i = 0; i < 16; i++) { + command[i] = (i < MAX_SERVO_CHANNELS) ? servo_out[i] : 0.0; } - std::cout << servo_out[MAX_SERVO_CHANNELS - 1] << "]" << std::endl; -// #endif + _ap_online = true; + return 1; } + return 0; + } - if (!ap.ap_online) { - continue; + int SendSimData(double time_now_sec, + const Eigen::Vector3d &velNED, + const Eigen::Vector3d &posGeodetic, + const Eigen::Quaterniond &attitudeFrdNed, + const Eigen::Vector3d &accFrd, + const Eigen::Vector3d &gyroFrd, + const Eigen::Vector3d &airspeedFrd, + float /*temperature*/, + float /*staticPressure*/, + float /*diffPressure*/) + { + if (!_inited) { + return -1; + } + if (!_ap_online) { + return 0; } + double rangefinder[6] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2}; + _libAP.setRangefinder(rangefinder, 6); + _libAP.setAirspeed(airspeedFrd.norm()); + _libAP.setWindvane(0.0f, 0.0f); + + double lat_deg = posGeodetic.x(); + double lon_deg = posGeodetic.y(); + double alt_m = posGeodetic.z(); + double gx = gyroFrd.x(); + double gy = gyroFrd.y(); + double gz = gyroFrd.z(); + double ax = accFrd.x(); + double ay = accFrd.y(); + double az = accFrd.z(); + + Eigen::Vector3d eul = attitudeFrdNed.toRotationMatrix().eulerAngles(0, 1, 2); + double roll = eul(0); + double pitch = eul(1); + double yaw = eul(2); + + double vx = velNED.x(); + double vy = velNED.y(); + double vz = velNED.z(); + + _libAP.SendState(time_now_sec, gx, gy, gz, ax, ay, az, + lat_deg, lon_deg, alt_m, + roll, pitch, yaw, + vx, vy, vz); + return 0; + } - // example rangefinder data - double rangefinder_example[] = {1, 2, 3, 4, 5, 6}; +static inline void process(const Eigen::Vector3d &velNED, + const Eigen::Vector3d &geodetic_position, + const Eigen::Quaterniond &attitudeFrdNed, + const Eigen::Vector3d &accFrd, + const Eigen::Vector3d &gyroFrd, + const Eigen::Vector3d &airspeedFrd, + float temperatureCelcius, + float staticPressureHpa, + float diffPressureHPa, + std::vector &actuators, + uint8_t &is_armed) +{ + static ArdupilotJsonInterface flight_stack; + if (!flight_stack._inited) { + flight_stack.Init("127.0.0.1", 9002); + return; + } + + double crnt_time = std::chrono::duration_cast>( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + uint64_t time_usec = static_cast(crnt_time * 1.0e6); + + bool isArmed; + std::array servo_cmd{}; + if (flight_stack.Receive(false, isArmed, servo_cmd) == 1) { + // Create a temporary array for normalized servo outputs + std::array actuators_raw{}; + + for (size_t i = 0; i < 16; i++) { + double pwm = servo_cmd[i]; + double norm = 0.0; + + norm = (pwm - 1500.0) / 500.0; // control surfaces in [-1..+1] + if (norm < -1.0) norm = -1.0; + if (norm > 1.0) norm = 1.0; + if (i == 2) + if (norm < 0.0) norm = 0.0; + + actuators_raw[i] = norm; + } - // set the optionals - ap.setAirspeed(1); - ap.setWindvane(1 , 1); - ap.setRangefinder(rangefinder_example, 6); + actuators[2] = actuators_raw[3]; // Rudder + actuators[4] = actuators_raw[2]; // Pusher + actuators[5] = -actuators_raw[0]; // Left Aileron + actuators[6] = actuators_raw[0]; // Right Aileron + actuators[7] = actuators_raw[1]; // Elevator - // send with the required - ap.SendState(timestamp, - 0, 0, 0, // gyro - 0, 0, -9.81, // accel - 0, 0, 0, // position - 0, 0, 0, // attitude - 0, 0, 0); // velocity + // for (auto i = 0; i < actuators.size(); ++i) { + // std::cout << actuators[i] << " "; + // } - usleep(1000); // run this example at about 1000 Hz. Realistically it is about 800 Hz. + is_armed = isArmed ? 1 : 2; } - return 0; + + double time_sec = static_cast(time_usec) / 1e6; + flight_stack.SendSimData(time_sec, + velNED, + geodetic_position, + attitudeFrdNed, + accFrd, + gyroFrd, + airspeedFrd, + temperatureCelcius, + staticPressureHpa, + diffPressureHPa); } -#endif // ARDUPILOT_JSON_INTERFACE_ARDUPILOT_JSON_INTERFACE_HPP +private: + libAP_JSON _libAP; + bool _inited; + bool _ap_online; +}; + +#endif diff --git a/src/Simulator/src/main.cpp b/src/Simulator/src/main.cpp index 19ad80c..57d6490 100644 --- a/src/Simulator/src/main.cpp +++ b/src/Simulator/src/main.cpp @@ -126,7 +126,34 @@ void threadRos() { } void threadArdupilotJson() { - ardupilot_json_interface(); + const auto period = std::chrono::microseconds(1000000 / 500); // 500 Hz + + while (true) { + auto start_time = std::chrono::high_resolution_clock::now(); + + float temperatureKelvin; + float staticPressureHpa; + float diffPressureHPa; + sim->atmosphere(&temperatureKelvin, &staticPressureHpa, &diffPressureHPa); + + ArdupilotJsonInterface::process( + sim->linearVelocityNed(), + /*sim->geoPosition()*/ sim->positionNed(), + sim->attitudeFrdNed(), + // sim->magneticFieldWithNoiseFrd(), + sim->getAccelFrd(), + sim->getGyroFrd(), + sim->getAirspeedFrd(), + temperatureKelvin - 273.15, + staticPressureHpa, + diffPressureHPa, + + setpoints, + (uint8_t&)arming_status + ); + auto end_time = std::chrono::high_resolution_clock::now(); + std::this_thread::sleep_for(period - (end_time - start_time)); + } } int main(int argc, char* argv[]) {