Sadly, i cant share much about my project, but I’ve made some quick edits to my code for just this json communication in case in might help someone.
Note c++ is not my strongest language and that this is still half finished, but it works thus far.
The first file just defines some types for easy access to the data later, the second has the implementation of communication and the main method.
Info_Types.h:
// holds roll,pitch, and yaw as well as angular speed of each
struct Attitude {
// in radians
float roll, pitch;
//radians from north
float yaw;
// in rad/s
float rollSpeed, pitchSpeed, yawSpeed;
Attitude() {
Attitude(100.0f, 100.0f, 100.0f, 100.0f, 100.0f, 100.0f);
}
Attitude(float r, float p, float y, float rs, float ps, float ys) {
roll = r;
pitch = p;
yaw = y;
rollSpeed = rs;
pitchSpeed = ps;
yawSpeed = ys;
}
};
// holds data from accelerometers and gyroscopes
struct IMU {
// in m/s/s
float xacc, yacc, zacc;
// in rad/s
float xgyr, ygyr, zgyr;
IMU(){
IMU(-1.0f,-1.0f,-1.0f,-1.0f,-1.0f,-1.0f);
};
IMU(int xa, int ya, int za, int xg, int yg, int zg) {
//starts in cm/s/s
xacc = 0.01 * (float) xa;
yacc = 0.01 * (float) ya;
zacc = 0.01 * (float) za;
xgyr = xg * 0.001;
ygyr = yg * 0.001;
zgyr = zg * 0.001;
};
};
struct Rangefinder {
// in meters
float range;
// raw voltage if available, zero otherwise
float voltage;
Rangefinder () {
Rangefinder(-1.0f,-1.0f);
};
Rangefinder(float r, float v) {
range = r;
voltage = v;
};
};
// scaled pressure
struct Barometer {
// in hectopascals
float absolutePressure;
// in hectopascals
float diffFromGroundPressure;
// in celcius
float temperature;
Barometer() {
Barometer(-1,-1, -1);
};
Barometer(float p, float d, float temp) {
absolutePressure = p;
diffFromGroundPressure = d;
// starts in centi-degrees, so convert
temperature = temp / 100;
};
};
// has basic flight info that is usually displayed in a HUD
struct VfrHud {
// in degrees
int heading;
// throttle the motors are currently running at from 0 - 100.
// 0 is full throttle down, 100 full throttle up, 50 for neutral
// note that these values are effected by the input gain
int throttle;
//in m/s
float speed;
// in m
float altitude;
// in m/s
float climbRate;
VfrHud(){};
VfrHud(int hdg, int throt, float spd, float alt, float clmbrt) {
heading = hdg;
throttle = throt;
speed = spd;
altitude = alt;
climbRate = clmbrt;
};
};
struct GPSData {
// in degrees
float heading;
// distance from starting point in meters
float relativeAltitude;
// in m/s
float velocity;
// [x,y,z] in m/s
float velocityComponents[3];
// in 1E7 degrees
long lat, lon;
GPSData() {};
GPSData(float head, float relAlt, float v, float vx, float vy, float vz, long la, long ln) {
heading = head * 0.01f; // convert from centi degrees
relativeAltitude = relAlt * 0.001f; // convert from mm
// convert from cm to m
velocity = v * 0.01f;
velocityComponents[0] = vx * 0.01f;
velocityComponents[1] = vy * 0.01f;
velocityComponents[2] = vz * 0.01f;
lat = la;
lon = ln;
};
};
ReadIn.cpp :
/*
retrieved json example here:
https://gist.github.com/connormanning/41efa6075515019e499c
compile with:
g++ readIn.cpp -ljsoncpp -lcurl -o reader
command to launch sitl:
Tools/autotest/sim_vehicle.py -l 33.810313,-118.393000,10,270 --out=udp:0.0.0.0:14550 -v ArduSub --no-mavproxy
command to launch mav-proxy:
mavproxy.py --master=tcp:0.0.0.0:5760 --sitl=0.0.0.0:5501 --out=udpout:0.0.0.0:14551 --out=udpout:0.0.0.0:14550
then use mavproxy commands:
module load restserver
restserver start
restserver address 0.0.0.0:5001
*/
#include <cstdint>
#include <iostream>
#include <memory>
#include <string>
#include <curl/curl.h>
#include <jsoncpp/json/json.h>
#include <sys/types.h>
#include <stdio.h>
#include "Info_Types.h"
/* QNX timer version */
#if (defined __QNX__) | (defined __QNXNTO__)
u_int64_t microsSinceEpoch()
{
struct timespec time;
uint64_t micros = 0;
clock_gettime(CLOCK_REALTIME, &time);
micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;
return micros;
}
#else
u_int64_t microsSinceEpoch()
{
struct timeval tv;
u_int64_t micros = 0;
gettimeofday(&tv, NULL);
micros = ((u_int64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
return micros;
}
#endif
using namespace std;
namespace
{
size_t callback(
const char* in,
size_t size,
size_t num,
string* out)
{
const size_t totalBytes(size * num);
out->append(in, totalBytes);
return totalBytes;
}
}
const string baseURL = "0.0.0.0:5001/rest/mavlink/";
Json::Value getThing(string whatToGet)
{
const string url(baseURL + whatToGet);
CURL* curl = curl_easy_init();
// Set remote URL.
curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
// Don't bother trying IPv6, which would increase DNS resolution time.
curl_easy_setopt(curl, CURLOPT_IPRESOLVE, CURL_IPRESOLVE_V4);
// Don't wait forever, time out after 10 seconds.
curl_easy_setopt(curl, CURLOPT_TIMEOUT, 10);
// Follow HTTP redirects if necessary.
curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L);
// Response information.
long httpCode(0);
std::unique_ptr<string> httpData(new string());
// Hook up data handling function.
curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, callback);
// Hook up data container (will be passed as the last parameter to the
// callback handling function). Can be any pointer type, since it will
// internally be passed as a void pointer.
curl_easy_setopt(curl, CURLOPT_WRITEDATA, httpData.get());
// Run our HTTP GET command, capture the HTTP response code, and clean up.
curl_easy_perform(curl);
curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &httpCode);
curl_easy_cleanup(curl);
if (httpCode == 200)
{
// std::cout << "\nGot successful response from " << url << std::endl;
// Response looks good - done using Curl now. Try to parse the results
// and print them out.
Json::Value jsonData;
Json::Reader jsonReader;
if (jsonReader.parse(*httpData.get(), jsonData))
{
// std::cout << "Successfully parsed JSON data" << std::endl;
cout << "\nJSON data received:" << endl;
cout << jsonData.toStyledString() << endl;
return jsonData;
}
else
{
cout << "Could not parse HTTP data as JSON" << endl;
cout << "HTTP data was:\n" << *httpData.get() << endl;
return Json::Value();
}
}
else
{
cout << "Couldn't GET from " << url << " - exiting" << endl;
return Json::Value();
}
// return 0;
}
// updates the attitude parameters
bool getAttitude (Attitude &a) {
Json::Value att = getThing("ATTITUDE");
if (att.empty()) {
return false;
}
a = Attitude(
stof(att["roll"].asString()),
stof(att["pitch"].asString()),
stof(att["yaw"].asString()),
stof(att["rollspeed"].asString()),
stof(att["pitchspeed"].asString()),
stof(att["yawspeed"].asString())
);
return true;
}
bool getIMU(bool raw, IMU &imu) {
Json::Value data = getThing(raw ? "RAW_IMU" : "SCALED_IMU2");
if (data.empty()) {
return false;
}
imu = IMU (
stoi(data["xacc"].asString()),
stoi(data["yacc"].asString()),
stoi(data["zacc"].asString()),
stoi(data["xgyro"].asString()),
stoi(data["ygyro"].asString()),
stoi(data["zgyro"].asString())
);
return true;
}
bool getRawImu (IMU &imu) {
return getIMU(true, imu);
}
bool getScaledImu (IMU &imu) {
return getIMU(false, imu);
}
bool getRangefinder(Rangefinder &rngfnd) {
Json::Value data = getThing("RANGEFINDER");
if (data.empty()) {
return false;
}
rngfnd = Rangefinder(
stof(data["distance"].asString()),
stof(data["voltage"].asString())
);
return true;
}
int main () {
u_int64_t total = 0;
int samples = 10;
for (int i = 0; i < samples; i++) {
u_int64_t start = microsSinceEpoch();
Attitude att;
if (getAttitude(att)) {
printf("Attitude: \n");
printf("roll: %f, pitch: %f, yaw: %f\n", att.roll, att.pitch, att.yaw);
printf("rollSpeed: %f, pitchSpeed: %f, yawSpeed: %f\n", att.rollSpeed, att.pitchSpeed, att.yawSpeed);
}
total += (microsSinceEpoch() - start);
}
printf("\nAverage time in micro seconds: %lu \n", (total / samples));
}
Sory, there’s a bit of bloat in there.
When I tried to use
--cmd="module load restserver; restserver start; restserver address 0.0.0.0:5001;"
as an argument when starting mavProxy, it would throw and error, work, but then the restserver would stay up even if i closed mavProxy. As such, i just use the console to do commands manually for now.
Hope this helps someone 
