Skip to content

Commit

Permalink
basic ArduPlane JSON integration
Browse files Browse the repository at this point in the history
  • Loading branch information
@frontw
frontw committed Jan 8, 2025
1 parent 3e33110 commit 6fb6849
Show file tree
Hide file tree
Showing 2 changed files with 192 additions and 53 deletions.
216 changes: 164 additions & 52 deletions src/ArdupilotInterface/include/ArdupilotInterface/ardupilot_json_interface.hpp
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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 <http://www.gnu.org/licenses/>.
*
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/

// 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 <time.h>
#include <chrono>
#include <stdlib.h>

#include <array>
#include <cmath>
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <Eigen/Dense>
#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::microseconds>(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<double, 16> &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<double> &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::duration<double>>(
std::chrono::system_clock::now().time_since_epoch())
.count();
uint64_t time_usec = static_cast<uint64_t>(crnt_time * 1.0e6);

bool isArmed;
std::array<double, 16> servo_cmd{};
if (flight_stack.Receive(false, isArmed, servo_cmd) == 1) {
// Create a temporary array for normalized servo outputs
std::array<double, 16> 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<double>(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
29 changes: 28 additions & 1 deletion src/Simulator/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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[]) {
Expand Down

0 comments on commit 6fb6849

Please sign in to comment.