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[]) {