Skip to content

Instantly share code, notes, and snippets.

@max-verem

max-verem/lab8_fdm_proxy_async.c

Created December 10, 2025 18:05
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save max-verem/e30f94366b99315869fb870a648363be to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save max-verem/e30f94366b99315869fb870a648363be to your computer and use it in GitHub Desktop.
Download ZIP
lab #8 proxy
Raw
lab8_fdm_proxy_async.c
#define DEFAULT_OUTPUT_PWM_FRAME_RATE 1000
#define DEFAULT_ATTITUDE 120.0
/*
will listen for UDP packets:
9001 - Betaflight's PwmOut UDP link to RF9
9002 - Betaflight's PwmOut UDP link to gazebo
will send
127.0.0.1:9003 - FDM packets to Betaflight
host:ip - PWM packets for ardupilot_gazebo plugin
compile:
sudo apt install libcjson-dev
gcc -Wall -g -ggdb -O0 lab8_fdm_proxy_async.c -o lab8_fdm_proxy_async -lcjson -lm
run:
gz topic -t /world/iris_runway/model/iris_with_gimbal_lab2/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1"
DISPLAY= gz sim -v4 -s -r --headless-rendering iris_runway_lab2.sdf
sudo /usr/sbin/sysctl -w net.core.rmem_default=26214400
sudo /usr/sbin/sysctl -w net.core.rmem_max=26214400
./lab8_fdm_proxy_async 10.1.5.65 9102
./lab8_fdm_proxy_async 10.1.4.105 9102
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <inttypes.h>
#include <unistd.h>
#include <asm/termbits.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/timerfd.h>
#include <sys/time.h>
#include <pthread.h>
#include <errno.h>
#include <pthread.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <netinet/udp.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <math.h>
#include <cjson/cJSON.h>
/*
https://github.com/betaflight/betaflight/blob/4.5.3/src/main/target/SITL/target.h
*/
#define SIMULATOR_MAX_RC_CHANNELS 16
#define SIMULATOR_MAX_PWM_CHANNELS 16
typedef struct {
double timestamp; // in seconds
double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se
double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256
double imu_orientation_quat[4]; //w, x, y, z
double velocity_xyz[3]; // m/s, earth frame
double position_xyz[3]; // meters, NED from origin
double pressure;
} fdm_packet;
typedef struct {
double timestamp; // in seconds
uint16_t channels[SIMULATOR_MAX_RC_CHANNELS]; // RC channels
} rc_packet;
typedef struct {
float motor_speed[4]; // normal: [0.0, 1.0], 3D: [-1.0, 1.0]
} servo_packet;
typedef struct {
uint16_t motorCount; // Count of motor in the PWM output.
float pwm_output_raw[SIMULATOR_MAX_PWM_CHANNELS]; // Raw PWM from 1100 to 1900
} servo_packet_raw;
/*
https://github.com/ArduPilot/ardupilot_gazebo/blob/main/include/ArduPilotPlugin.hh
*/
// The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h.
struct servo_packet_16 {
uint16_t magic; // 18458 expected magic value
uint16_t frame_rate;
uint32_t frame_count;
uint16_t pwm[16];
};
struct servo_packet_32 {
uint16_t magic; // 29569 expected magic value
uint16_t frame_rate;
uint32_t frame_count;
uint16_t pwm[32];
};
// ----------------------------------------------------------------
/*
127.0.0.1:9001
127.0.0.1:9002
127.0.0.1:9003
*/
int create_udp_socket(char* addr, int port)
{
int fd, r;
fd = socket(AF_INET, SOCK_DGRAM, 0);
if(fd < 0)
{
r = errno;
fprintf(stderr, "%s: socket(AF_INET, SOCK_DGRAM, 0) failed: r=%d (%s)\n", __FUNCTION__, r, strerror(r));
close(fd);
return -r;
};
if(port)
{
struct sockaddr_in s_addr = {0};
s_addr.sin_family = AF_INET;
s_addr.sin_addr.s_addr = htonl(!addr ? INADDR_ANY : inet_addr(addr));
s_addr.sin_port = htons(port);
r = bind(fd, (struct sockaddr *) &s_addr, sizeof(s_addr));
if(fd < 0)
{
r = errno;
fprintf(stderr, "%s: bind(%s, %d) failed: r=%d (%s)\n", __FUNCTION__,
inet_ntoa(s_addr.sin_addr), ntohs(s_addr.sin_port), r, strerror(r));
close(fd);
return -r;
};
};
return fd;
};
static struct sockaddr_in s_addr_gazebo = {0};
static struct sockaddr_in s_addr_betaflight = {0};
static int
output_pwm_frame_rate = DEFAULT_OUTPUT_PWM_FRAME_RATE,
f_exit = 0;
static int
fd_in_pwm_betaflight_gazebo = -1,
fd_in_pwm_betaflight_rf9 = -1,
fd_out_pwm_gazebo = -1,
fd_out_fdm_betaflight = -1;
static struct
{
int64_t ts_ns;
uint64_t
in_pwm_betaflight_gazebo,
in_pwm_betaflight_rf9,
out_pwm_gazebo,
in_fdm_gazebo,
out_fdm_betaflight;
} cnt_curr = {0}, cnt_prev = {0}, cnt_tmp = {0};
static void* proc_in_pwm_betaflight_rf9(void* p __attribute__((unused)));
static void* proc_in_pwm_betaflight_gazebo(void* p __attribute__((unused)));
static void* proc_out_pwm_gazebo(void* p __attribute__((unused)));
static void* proc_in_fdm_gazebo(void* p __attribute__((unused)));
static void in_pwm_timeout(int f_to_gazebo);
static void in_pwm_packet(servo_packet_raw* rf9, servo_packet* gazebo);
static int parse_json_fdm_packet(char *buf, fdm_packet *pkt);
static fdm_packet last_fdm = {0};
static servo_packet last_servo_packet = {0};
static servo_packet_raw last_servo_packet_raw = {0};
static pthread_spinlock_t lock;
int64_t get_clock_monotonic_raw_ns();
static double pkt_freq(int64_t t1, uint64_t v1, int64_t t2, uint64_t v2)
{
int64_t dT = t2 - t1;
uint64_t dV = v2 - v1;
if(!dT) return 0.0;
return (dV * 10000000000LL / dT) / 10.0;
};
// betaflight/src/main/sensors/barometer.c
double pressureToAltitude(const double pressure) // result in cm (0.01m)
{
return (1.0 - pow(pressure / 101325.0, 0.190295)) * 4433000.0;
}
double altitude_to_pressure(const double altitude)
{
return 101325.0 * pow(1 - 2.25577e-05 * altitude, 5.25588);
};
int main(int argc, char** argv)
{
pthread_t th_in_pwm_betaflight_rf9, th_in_pwm_betaflight_gazebo, th_out_pwm_gazebo, th_in_fdm_gazebo;
if(0)
{
double a, p;
double tests[] = {0.0, 0.1, 1.0, 10.0, 100.0, 1000.0, 2000.0, 4000.0, 11000, -1.0};
double *alts = tests;
fprintf(stderr, "pressureToAltitude(%f)=%f\n", 101325.0, pressureToAltitude(101325.0));
fprintf(stderr, "pressureToAltitude(%f)=%f\n", 22632.1, pressureToAltitude(22632.1));
while(*alts >= 0.0)
{
p = altitude_to_pressure(*alts);
a = pressureToAltitude(p);
fprintf(stderr, "v=%f, p=%f, a=%f\n", *alts, p, a);
alts++;
};
exit(1);
};
if(argc != 3)
{
fprintf(stderr, "Error: not enought arguments!\n\nUsage:\n\t%s <gazebo ip> <gazebo port>\n", argv[0]);
return 1;
};
s_addr_gazebo.sin_family = AF_INET;
s_addr_gazebo.sin_addr.s_addr = inet_addr(argv[1]);
s_addr_gazebo.sin_port = htons(atoi(argv[2]));
s_addr_betaflight.sin_family = AF_INET;
s_addr_betaflight.sin_addr.s_addr = inet_addr("127.0.0.1");
s_addr_betaflight.sin_port = htons(9003);
fd_in_pwm_betaflight_rf9 = create_udp_socket(NULL, 9001);
if(fd_in_pwm_betaflight_rf9 < 0)
goto ex;
fd_in_pwm_betaflight_gazebo = create_udp_socket(NULL, 9002);
if(fd_in_pwm_betaflight_gazebo < 0)
goto ex;
fd_out_pwm_gazebo = create_udp_socket(NULL, 0);
if(fd_out_pwm_gazebo < 0)
goto ex;
fd_out_fdm_betaflight = create_udp_socket(NULL, 0);
if(fd_out_fdm_betaflight < 0)
goto ex;
pthread_spin_init(&lock, 0);
pthread_create(&th_in_pwm_betaflight_rf9, NULL, proc_in_pwm_betaflight_rf9, NULL);
pthread_create(&th_in_pwm_betaflight_gazebo, NULL, proc_in_pwm_betaflight_gazebo, NULL);
pthread_create(&th_in_fdm_gazebo, NULL, proc_in_fdm_gazebo, NULL);
pthread_create(&th_out_pwm_gazebo, NULL, proc_out_pwm_gazebo, NULL);
while(!f_exit)
{
static int i = 0;
static const char idle[] = "|/-\\";
usleep(250000);
cnt_curr.ts_ns = get_clock_monotonic_raw_ns();
pthread_spin_lock(&lock);
cnt_tmp = cnt_curr;
pthread_spin_unlock(&lock);
#define PKT_FREQ(V) pkt_freq(cnt_prev.ts_ns, cnt_prev.V, cnt_tmp.ts_ns, cnt_tmp.V)
fprintf(stdout, "[%c]\n", idle[(i++) % (sizeof(idle) - 1)]);
fprintf(stdout, " PWM PKT IN: %8" PRIu64 " / %8" PRIu64 " | PWM PKT OUT: %8" PRIu64 "\n",
cnt_tmp.in_pwm_betaflight_gazebo, cnt_tmp.in_pwm_betaflight_rf9, cnt_tmp.out_pwm_gazebo);
fprintf(stdout, " %7.2f Hz / %7.2f Hz | %7.2f Hz\n\n",
PKT_FREQ(in_pwm_betaflight_gazebo), PKT_FREQ(in_pwm_betaflight_rf9), PKT_FREQ(out_pwm_gazebo));
fprintf(stdout, " IN PWM RAW: %10.3f %10.3f %10.3f %10.3f \n\n",
last_servo_packet_raw.pwm_output_raw[0], last_servo_packet_raw.pwm_output_raw[1],
last_servo_packet_raw.pwm_output_raw[2], last_servo_packet_raw.pwm_output_raw[3]);
fprintf(stdout, " FDM PKT IN: %8" PRIu64 " | FDM PKT OUT: %8" PRIu64 "\n",
cnt_tmp.in_fdm_gazebo, cnt_tmp.out_fdm_betaflight);
fprintf(stdout, " %7.2f Hz | %7.2f Hz\n\n",
PKT_FREQ(in_fdm_gazebo), PKT_FREQ(out_fdm_betaflight));
fprintf(stdout, " TIMESTAMP: %10.3f\n", last_fdm.timestamp);
fprintf(stdout, " POS XYZ: [ %10.3f, %10.3f, %10.3f ]\n",
last_fdm.position_xyz[0], last_fdm.position_xyz[1], last_fdm.position_xyz[2]);
fprintf(stdout, " VEL XYZ: [ %10.3f, %10.3f, %10.3f ]\n",
last_fdm.velocity_xyz[0], last_fdm.velocity_xyz[1], last_fdm.velocity_xyz[2]);
fprintf(stdout, " QUAT WXYZ: [ %10.3f, %10.3f, %10.3f, %10.3f ]\n",
last_fdm.imu_orientation_quat[0], last_fdm.imu_orientation_quat[1], last_fdm.imu_orientation_quat[2], last_fdm.imu_orientation_quat[3]);
fprintf(stdout, " ACCEL XYZ: [ %10.3f, %10.3f, %10.3f ]\n",
last_fdm.imu_linear_acceleration_xyz[0], last_fdm.imu_linear_acceleration_xyz[1], last_fdm.imu_linear_acceleration_xyz[2]);
fprintf(stdout, " ANG VEL RPY: [ %10.3f, %10.3f, %10.3f ]\n",
last_fdm.imu_angular_velocity_rpy[0], last_fdm.imu_angular_velocity_rpy[1], last_fdm.imu_angular_velocity_rpy[2]);
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
fprintf(stdout, "\033[1A");
cnt_prev = cnt_tmp;
};
shutdown(fd_in_pwm_betaflight_gazebo, SHUT_RDWR);
shutdown(fd_in_pwm_betaflight_rf9, SHUT_RDWR);
shutdown(fd_out_pwm_gazebo, SHUT_RDWR);
shutdown(fd_out_fdm_betaflight, SHUT_RDWR);
pthread_join(th_in_pwm_betaflight_rf9, NULL);
pthread_join(th_in_pwm_betaflight_gazebo, NULL);
pthread_join(th_out_pwm_gazebo, NULL);
pthread_join(th_in_fdm_gazebo, NULL);
pthread_spin_destroy(&lock);
ex:
if(fd_in_pwm_betaflight_gazebo > 0) close(fd_in_pwm_betaflight_gazebo);
if(fd_in_pwm_betaflight_rf9 > 0) close(fd_in_pwm_betaflight_rf9);
if(fd_out_pwm_gazebo > 0) close(fd_out_pwm_gazebo);
if(fd_out_fdm_betaflight > 0) close(fd_out_fdm_betaflight);
return 0;
};
static void* proc_in_pwm_betaflight_rf9(void* p __attribute__((unused)))
{
int r;
fd_set fds;
char buf[2048];
struct timeval tv;
while(!f_exit)
{
/* prepare to check if data present in buffer */
FD_ZERO (&fds);
FD_SET (fd_in_pwm_betaflight_rf9, &fds);
tv.tv_sec = 0; /* Timeout 'sec'. */
tv.tv_usec = 500000; /* Timeout 'usec'. */
/* check */
r = select(fd_in_pwm_betaflight_rf9 + 1, &fds, NULL, NULL, &tv);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: select() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
f_exit++;
return NULL;
};
if(!r)
{
in_pwm_timeout(0);
continue;
};
r = recv(fd_in_pwm_betaflight_rf9, buf, sizeof(buf), 0);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: recv() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
f_exit++;
return NULL;
};
if(sizeof(servo_packet_raw) != r)
{
fprintf(stderr, "%s: received %d bytes, sizeof(servo_packet_raw) == %lu", __FUNCTION__, r, sizeof(servo_packet_raw));
continue;
};
// here we process a packets sent to RF9 from Betaflight
in_pwm_packet((servo_packet_raw*)buf, NULL);
cnt_curr.in_pwm_betaflight_rf9++;
};
return NULL;
};
static void* proc_in_pwm_betaflight_gazebo(void* p __attribute__((unused)))
{
int r;
fd_set fds;
char buf[2048];
struct timeval tv;
while(!f_exit)
{
/* prepare to check if data present in buffer */
FD_ZERO (&fds);
FD_SET (fd_in_pwm_betaflight_gazebo, &fds);
tv.tv_sec = 0; /* Timeout 'sec'. */
tv.tv_usec = 500000; /* Timeout 'usec'. */
/* check */
r = select(fd_in_pwm_betaflight_gazebo + 1, &fds, NULL, NULL, &tv);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: select() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
f_exit++;
return NULL;
};
if(!r)
{
in_pwm_timeout(1);
continue;
};
r = recv(fd_in_pwm_betaflight_gazebo, buf, sizeof(buf), 0);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: recv() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
f_exit++;
return NULL;
};
if(sizeof(servo_packet) != r)
{
fprintf(stderr, "%s: received %d bytes, sizeof(servo_packet) == %lu", __FUNCTION__, r, sizeof(servo_packet));
continue;
};
// here we process a packets sent to Gazebo from Betaflight
in_pwm_packet(NULL, (servo_packet*)buf);
cnt_curr.in_pwm_betaflight_gazebo++;
};
return NULL;
};
static void* proc_in_fdm_gazebo(void* p __attribute__((unused)))
{
int r, fd_in_fdm_gazebo = fd_out_pwm_gazebo;
fd_set fds;
char buf[2048];
struct timeval tv;
while(!f_exit)
{
fdm_packet pkt = {0};
/* prepare to check if data present in buffer */
FD_ZERO (&fds);
FD_SET (fd_in_fdm_gazebo, &fds);
tv.tv_sec = 0; /* Timeout 'sec'. */
tv.tv_usec = 500000; /* Timeout 'usec'. */
/* check */
r = select(fd_in_fdm_gazebo + 1, &fds, NULL, NULL, &tv);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: select() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
f_exit++;
return NULL;
};
if(!r)
continue;
r = recv(fd_in_fdm_gazebo, buf, sizeof(buf), 0);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: recv() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
f_exit++;
return NULL;
};
buf[r] = 0;
r = parse_json_fdm_packet(buf, &pkt);
if(r < 0)
continue;
pthread_spin_lock(&lock);
cnt_curr.in_fdm_gazebo++;
last_fdm = pkt;
pthread_spin_unlock(&lock);
};
return NULL;
};
static void send_fdm_to_betaflight()
{
fdm_packet pkt = {0};
pthread_spin_lock(&lock);
pkt = last_fdm;
cnt_curr.out_fdm_betaflight++;
pthread_spin_unlock(&lock);
sendto(fd_out_fdm_betaflight, &pkt, sizeof(pkt), 0, (struct sockaddr *)&s_addr_betaflight, sizeof(s_addr_betaflight));
};
static void in_pwm_timeout(int f_to_gazebo)
{
if(f_to_gazebo)
return;
send_fdm_to_betaflight();
};
static void in_pwm_packet(servo_packet_raw* rf9, servo_packet* gz)
{
/*
if(rf9)
fprintf(stderr, "\nservo_packet_raw: %f, %f, %f, %f\n",
rf9->pwm_output_raw[0], rf9->pwm_output_raw[1], rf9->pwm_output_raw[2], rf9->pwm_output_raw[3]);
else if(gz)
fprintf(stderr, "\nservo_packet: %f, %f, %f, %f\n",
gz->motor_speed[0], gz->motor_speed[1], gz->motor_speed[2], gz->motor_speed[3]);
servo_packet: 0.000000, 0.000000, 0.000000, 0.000000
servo_packet_raw: 1000.000000, 1000.000000, 1000.000000, 1000.000000
*/
pthread_spin_lock(&lock);
if(rf9)
last_servo_packet_raw = *rf9;
if(gz)
last_servo_packet = *gz;
pthread_spin_unlock(&lock);
if(rf9)
send_fdm_to_betaflight();
};
static int parse_json_fdm_packet(char *json_buffer, fdm_packet *pkt)
{
/*
{
"timestamp":909.144,
"imu":{
"gyro":[-0.043994875844701,-0.000572378842194773,-0.007185674781765455],
"accel_body":[0.010770732557861375,0.5601342938212403,-9.739123093417417]
},
"position":[100.00533744764076,0.0007287157985803391,-10.217000650458129],
"quaternion":[0.999985203396347,-0.001301780858639356,0.00015294113943493633,-0.005279674608404283],
"velocity":[-0.005383170717085117,0.015871287066207209,0.00602523647475713]
}
*/
int r = -1, i;
/* parse json */
cJSON *json_root = cJSON_Parse(json_buffer);
if(!json_root)
{
const char *json_error_ptr = cJSON_GetErrorPtr();
if(json_error_ptr)
fprintf(stderr, "%s: cJSON_Parse failed: %s\n", __FUNCTION__, json_error_ptr);
return -1;
};
// timestamp
cJSON *json_timestamp = cJSON_GetObjectItemCaseSensitive(json_root, "timestamp");
if(!cJSON_IsNumber(json_timestamp))
{
fprintf(stderr, "%s: cJSON_IsNumber(timestamp) is false\n", __FUNCTION__);
goto ex1;
}
pkt->timestamp = cJSON_GetNumberValue(json_timestamp);
// imu
cJSON *json_imu = cJSON_GetObjectItemCaseSensitive(json_root, "imu");
if(!cJSON_IsObject(json_imu))
{
fprintf(stderr, "%s: cJSON_IsObject(json_imu) is false\n", __FUNCTION__);
goto ex1;
}
else
{
// gyro
cJSON *json_imu_gyro = cJSON_GetObjectItemCaseSensitive(json_imu, "gyro");
if(!cJSON_IsArray(json_imu_gyro))
{
fprintf(stderr, "%s: cJSON_IsArray(json_imu_gyro) is false\n", __FUNCTION__);
goto ex1;
}
else
{
r = cJSON_GetArraySize(json_imu_gyro);
if(3 != r)
{
fprintf(stderr, "%s: cJSON_GetArraySize(json_imu_gyro) == %d\n", __FUNCTION__, r);
goto ex1;
};
for(i = 0; i < 3; i++)
{
cJSON *json_imu_gyro_item;
json_imu_gyro_item = cJSON_GetArrayItem(json_imu_gyro, i);
if(!cJSON_IsNumber(json_imu_gyro_item))
{
fprintf(stderr, "%s: cJSON_IsNumber(json_imu_gyro_item[%d]) is false\n", __FUNCTION__, i);
goto ex1;
};
pkt->imu_angular_velocity_rpy[i] = cJSON_GetNumberValue(json_imu_gyro_item);
};
};
// accel_body
cJSON *json_imu_accel = cJSON_GetObjectItemCaseSensitive(json_imu, "accel_body");
if(!cJSON_IsArray(json_imu_accel))
{
fprintf(stderr, "%s: cJSON_IsArray(json_imu_accel) is false\n", __FUNCTION__);
goto ex1;
}
else
{
r = cJSON_GetArraySize(json_imu_accel);
if(3 != r)
{
fprintf(stderr, "%s: cJSON_GetArraySize(json_imu_accel) == %d\n", __FUNCTION__, r);
goto ex1;
};
for(i = 0; i < 3; i++)
{
cJSON *json_imu_accel_item;
json_imu_accel_item = cJSON_GetArrayItem(json_imu_accel, i);
if(!cJSON_IsNumber(json_imu_accel_item))
{
fprintf(stderr, "%s: cJSON_IsNumber(json_imu_accel_item[%d]) is false\n", __FUNCTION__, i);
goto ex1;
};
pkt->imu_linear_acceleration_xyz[i] = cJSON_GetNumberValue(json_imu_accel_item);
};
};
};
// quaternion
cJSON *json_quaternion = cJSON_GetObjectItemCaseSensitive(json_root, "quaternion");
if(!cJSON_IsArray(json_quaternion))
{
fprintf(stderr, "%s: cJSON_IsArray(quaternion) is false\n", __FUNCTION__);
goto ex1;
}
else
{
r = cJSON_GetArraySize(json_quaternion);
if(4 != r)
{
fprintf(stderr, "%s: cJSON_GetArraySize(quaternion) == %d\n", __FUNCTION__, r);
goto ex1;
};
for(i = 0; i < 4; i++)
{
cJSON *json_quaternion_item;
json_quaternion_item = cJSON_GetArrayItem(json_quaternion, i);
if(!cJSON_IsNumber(json_quaternion_item))
{
fprintf(stderr, "%s: cJSON_IsNumber(json_quaternion_item[%d]) is false\n", __FUNCTION__, i);
goto ex1;
};
pkt->imu_orientation_quat[i] = cJSON_GetNumberValue(json_quaternion_item);
};
};
// velocity
cJSON *json_velocity = cJSON_GetObjectItemCaseSensitive(json_root, "velocity");
if(!cJSON_IsArray(json_velocity))
{
fprintf(stderr, "%s: cJSON_IsArray(velocity) is false\n", __FUNCTION__);
goto ex1;
}
else
{
r = cJSON_GetArraySize(json_velocity);
if(3 != r)
{
fprintf(stderr, "%s: cJSON_GetArraySize(velocity) == %d\n", __FUNCTION__, r);
goto ex1;
};
for(i = 0; i < 3; i++)
{
cJSON *json_velocity_item;
json_velocity_item = cJSON_GetArrayItem(json_velocity, i);
if(!cJSON_IsNumber(json_velocity_item))
{
fprintf(stderr, "%s: cJSON_IsNumber(json_position_item[%d]) is false\n", __FUNCTION__, i);
goto ex1;
};
pkt->velocity_xyz[i] = cJSON_GetNumberValue(json_velocity_item);
};
};
// position
cJSON *json_position = cJSON_GetObjectItemCaseSensitive(json_root, "position");
if(!cJSON_IsArray(json_position))
{
fprintf(stderr, "%s: cJSON_IsArray(position) is false\n", __FUNCTION__);
goto ex1;
}
else
{
r = cJSON_GetArraySize(json_position);
if(3 != r)
{
fprintf(stderr, "%s: cJSON_GetArraySize(position) == %d\n", __FUNCTION__, r);
goto ex1;
};
for(i = 0; i < 3; i++)
{
cJSON *json_position_item;
json_position_item = cJSON_GetArrayItem(json_position, i);
if(!cJSON_IsNumber(json_position_item))
{
fprintf(stderr, "%s: cJSON_IsNumber(json_position_item[%d]) is false\n", __FUNCTION__, i);
goto ex1;
};
pkt->position_xyz[i] = cJSON_GetNumberValue(json_position_item);
};
};
// we will calculate pressure for further barameter using
pkt->pressure = altitude_to_pressure(DEFAULT_ATTITUDE - pkt->position_xyz[2]);
ex1:
cJSON_Delete(json_root);
return r;
};
static void* proc_out_pwm_gazebo(void* p __attribute__((unused)))
{
int fd, r;
struct itimerspec t;
fd = timerfd_create(CLOCK_MONOTONIC, 0);
if(fd < 0)
{
r = errno;
fprintf(stderr, "%s: timerfd_create() failed: r=%d (%s)\n", __FUNCTION__, r, strerror(r));
goto ex0;
};
t.it_value.tv_sec =
t.it_interval.tv_sec = 0;
t.it_value.tv_nsec =
t.it_interval.tv_nsec = 1000000000LL / output_pwm_frame_rate;
r = timerfd_settime(fd, 0, &t, NULL);
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: timerfd_settime() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
goto ex1;
};
while(!f_exit)
{
uint64_t v;
struct servo_packet_16 pwm = {0};
r = read(fd, &v, sizeof(uint64_t));
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: timerfd_settime() failed, r=%d (%s)", __FUNCTION__, r, strerror(r));
goto ex1;
};
pwm.magic = 18458;
pwm.frame_rate = output_pwm_frame_rate + 1;
pwm.frame_count = cnt_curr.out_pwm_gazebo++;
// https://ardupilot.org/copter/docs/connect-escs-and-motors.html
pthread_spin_lock(&lock);
pwm.pwm[3] = last_servo_packet_raw.pwm_output_raw[0];
pwm.pwm[0] = last_servo_packet_raw.pwm_output_raw[1];
pwm.pwm[1] = last_servo_packet_raw.pwm_output_raw[2];
pwm.pwm[2] = last_servo_packet_raw.pwm_output_raw[3];
pthread_spin_unlock(&lock);
r = sendto(fd_out_pwm_gazebo, &pwm, sizeof(pwm), 0, (struct sockaddr *)&s_addr_gazebo, sizeof(s_addr_gazebo));
if(r < 0)
{
r = errno;
fprintf(stderr, "%s: sendto(%s:%d) failed: r=%d (%s)\n",
__FUNCTION__, inet_ntoa(s_addr_gazebo.sin_addr), ntohs(s_addr_gazebo.sin_port), r, strerror(r));
goto ex1;
};
};
ex1:
close(fd);
ex0:
f_exit++;
return NULL;
};
int64_t get_timeval_timestamp_us(struct timeval *tv)
{
return ((int64_t)tv->tv_sec * 1000000LL) + (int64_t)tv->tv_usec;
};
int64_t get_current_timestamp_us()
{
struct timeval tv;
gettimeofday(&tv, NULL);
return get_timeval_timestamp_us(&tv);
};
int64_t get_clock_monotonic_raw_ns()
{
struct timespec tm;
clock_gettime(CLOCK_MONOTONIC_RAW, &tm);
return tm.tv_nsec + tm.tv_sec * 1000000000LL;
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment