src/main/target/SITL/sim/xplane.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <stdarg.h>
  30 #include <sys/socket.h>
  31 #include <arpa/inet.h>
  32 #include <netdb.h>
  33 #include <sys/types.h>
  34 #include <netinet/in.h>
  35 #include <pthread.h>
  36 #include <errno.h>
  37 #include <math.h>
  38
  39 #include "platform.h"
  40
  41 #include "target.h"
  42 #include "target/SITL/sim/xplane.h"
  43 #include "target/SITL/sim/simHelper.h"
  44 #include "fc/runtime_config.h"
  45 #include "drivers/time.h"
  46 #include "drivers/accgyro/accgyro_fake.h"
  47 #include "drivers/barometer/barometer_fake.h"
  48 #include "sensors/battery_sensor_fake.h"
  49 #include "sensors/acceleration.h"
  50 #include "drivers/pitotmeter/pitotmeter_fake.h"
  51 #include "drivers/compass/compass_fake.h"
  52 #include "drivers/rangefinder/rangefinder_virtual.h"
  53 #include "io/rangefinder.h"
  54 #include "common/utils.h"
  55 #include "common/maths.h"
  56 #include "flight/mixer.h"
  57 #include "flight/servos.h"
  58 #include "flight/imu.h"
  59 #include "io/gps.h"
  60 #include "rx/sim.h"
  61
  62 #define XP_PORT 49000
  63 #define XPLANE_JOYSTICK_AXIS_COUNT 8
  64
  65
  66 static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
  67 static uint8_t mappingCount;
  68
  69 static struct sockaddr_storage serverAddr;
  70 static socklen_t serverAddrLen;
  71 static int sockFd;
  72 static pthread_t listenThread;
  73 static bool initalized = false;
  74 static bool useImu = false;
  75
  76 static float lattitude = 0;
  77 static float longitude = 0;
  78 static float elevation = 0;
  79 static float agl = 0;
  80 static float local_vx = 0;
  81 static float local_vy = 0;
  82 static float local_vz = 0;
  83 static float groundspeed = 0;
  84 static float airspeed = 0;
  85 static float roll = 0;
  86 static float pitch = 0;
  87 static float yaw = 0;
  88 static float hpath = 0;
  89 static float accel_x = 0;
  90 static float accel_y = 0;
  91 static float accel_z = 0;
  92 static float gyro_x = 0;
  93 static float gyro_y = 0;
  94 static float gyro_z = 0;
  95 static float barometer = 0;
  96 static bool  hasJoystick = false;
  97 static float joystickRaw[XPLANE_JOYSTICK_AXIS_COUNT];
  98
  99 typedef enum
 100 {
 101     DREF_LATITUDE,
 102     DREF_LONGITUDE,
 103     DREF_ELEVATION,
 104     DREF_AGL,
 105     DREF_LOCAL_VX,
 106     DREF_LOCAL_VY,
 107     DREF_LOCAL_VZ,
 108     DREF_GROUNDSPEED,
 109     DREF_TRUE_AIRSPEED,
 110     DREF_POS_PHI,
 111     DREF_POS_THETA,
 112     DREF_POS_PSI,
 113     DREF_POS_HPATH,
 114     DREF_FORCE_G_AXI1,
 115     DREF_FORCE_G_SIDE,
 116     DREF_FORCE_G_NRML,
 117     DREF_POS_P,
 118     DREF_POS_Q,
 119     DREF_POS_R,
 120     DREF_POS_BARO_CURRENT_INHG,
 121     DREF_COUNT,
 122     DREF_HAS_JOYSTICK,
 123     DREF_JOYSTICK_VALUES_ROll,
 124     DREF_JOYSTICK_VALUES_PITCH,
 125     DREF_JOYSTICK_VALUES_THROTTLE,
 126     DREF_JOYSTICK_VALUES_YAW,
 127     DREF_JOYSTICK_VALUES_CH5,
 128     DREF_JOYSTICK_VALUES_CH6,
 129     DREF_JOYSTICK_VALUES_CH7,
 130     DREF_JOYSTICK_VALUES_CH8,
 131 } dref_t;
 132
 133 uint32_t xint2uint32 (uint8_t * buf)
 134 {
 135         return buf[3] << 24 | buf [2] << 16 | buf [1] << 8 | buf [0];
 136 }
 137
 138 float xflt2float (uint8_t * buf)
 139 {
 140         union {
 141                 float f;
 142                 uint32_t i;
 143         } v;
 144
 145         v.i = xint2uint32 (buf);
 146         return v.f;
 147 }
 148
 149 static void registerDref(dref_t id, char* dref, uint32_t freq)
 150 {
 151     char buf[413];
 152     memset(buf, 0, sizeof(buf));
 153
 154     strcpy(buf, "RREF");
 155     memcpy(buf + 5, &freq, 4);
 156     memcpy(buf + 9, &id, 4);
 157     memcpy(buf + 13, dref, strlen(dref) + 1);
 158
 159     sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen);
 160 }
 161
 162 static void sendDref(char* dref, float value)
 163 {
 164     char buf[509];
 165     strcpy(buf, "DREF");
 166     memcpy(buf + 5, &value, 4);
 167     memset(buf + 9, ' ', sizeof(buf) - 9);
 168     strcpy(buf + 9, dref);
 169
 170     sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen);
 171 }
 172
 173 static void* listenWorker(void* arg)
 174 {
 175     UNUSED(arg);
 176
 177     uint8_t buf[1024];
 178     struct sockaddr_storage remoteAddr;
 179     socklen_t slen = sizeof(remoteAddr);
 180     int recvLen;
 181
 182     while (true)
 183     {
 184
 185         float motorValue = 0;
 186         float yokeValues[3] = { 0 };
 187         int y = 0;
 188         for (int i = 0; i < mappingCount; i++) {
 189             if (y > 2) {
 190                 break;
 191             }
 192             if (pwmMapping[i] & 0x80) { // Motor
 193                 motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]);
 194             } else {
 195                 yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]);
 196                 y++;
 197             }
 198         }
 199
 200         sendDref("sim/operation/override/override_joystick", 1);
 201         sendDref("sim/cockpit2/engine/actuators/throttle_ratio_all", motorValue);
 202         sendDref("sim/joystick/yoke_roll_ratio", yokeValues[0]);
 203         sendDref("sim/joystick/yoke_pitch_ratio", yokeValues[1]);
 204         sendDref("sim/joystick/yoke_heading_ratio", yokeValues[2]);
 205         sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[0]", 0);
 206         sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[1]", 0);
 207         sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[2]", 0);
 208         sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[3]", 0);
 209         sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[4]", 0);
 210
 211         recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
 212         if (recvLen < 0 && errno != EWOULDBLOCK) {
 213             continue;
 214         }
 215
 216         if (strncmp((char*)buf, "RREF", 4) != 0) {
 217             continue;
 218         }
 219
 220         for (int i = 5; i < recvLen; i += 8) {
 221             dref_t dref = (dref_t)xint2uint32(&buf[i]);
 222             float value = xflt2float(&(buf[i + 4]));
 223
 224             switch (dref)
 225             {
 226                 case DREF_LATITUDE:
 227                     lattitude = value;
 228                     break;
 229
 230                 case DREF_LONGITUDE:
 231                     longitude = value;
 232                     break;
 233
 234                 case DREF_ELEVATION:
 235                     elevation = value;
 236                     break;
 237
 238                 case DREF_AGL:
 239                     agl = value;
 240                     break;
 241
 242                 case DREF_LOCAL_VX:
 243                     local_vx = value;
 244                     break;
 245
 246                 case DREF_LOCAL_VY:
 247                     local_vy = value;
 248                     break;
 249
 250                 case DREF_LOCAL_VZ:
 251                     local_vz = value;
 252                     break;
 253
 254                 case DREF_GROUNDSPEED:
 255                     groundspeed = value;
 256                     break;
 257
 258                 case DREF_TRUE_AIRSPEED:
 259                     airspeed = value;
 260                     break;
 261
 262                 case DREF_POS_PHI:
 263                     roll = value;
 264                     break;
 265
 266                 case DREF_POS_THETA:
 267                     pitch = value;
 268                     break;
 269
 270                 case DREF_POS_PSI:
 271                     yaw = value;
 272                     break;
 273
 274                 case DREF_POS_HPATH:
 275                     hpath = value;
 276                     break;
 277
 278                 case DREF_FORCE_G_AXI1:
 279                     accel_x = value;
 280                     break;
 281
 282                 case DREF_FORCE_G_SIDE:
 283                     accel_y = value;
 284                     break;
 285
 286                 case DREF_FORCE_G_NRML:
 287                     accel_z = value;
 288                     break;
 289
 290                 case DREF_POS_P:
 291                     gyro_x = value;
 292                     break;
 293
 294                 case DREF_POS_Q:
 295                     gyro_y = value;
 296                     break;
 297
 298                 case DREF_POS_R:
 299                     gyro_z = value;
 300                     break;
 301
 302                 case DREF_POS_BARO_CURRENT_INHG:
 303                     barometer = value;
 304                     break;
 305
 306                 case DREF_HAS_JOYSTICK:
 307                     hasJoystick = value >= 1 ? true : false;
 308                     break;
 309
 310                 case DREF_JOYSTICK_VALUES_ROll:
 311                     joystickRaw[0] = value;
 312                     break;
 313
 314                 case DREF_JOYSTICK_VALUES_PITCH:
 315                     joystickRaw[1] = value;
 316                     break;
 317
 318                 case DREF_JOYSTICK_VALUES_THROTTLE:
 319                     joystickRaw[2] = value;
 320                     break;
 321
 322                 case DREF_JOYSTICK_VALUES_YAW:
 323                     joystickRaw[3] = value;
 324                     break;
 325
 326                 case DREF_JOYSTICK_VALUES_CH5:
 327                     joystickRaw[4] = value;
 328                     break;
 329
 330                 case DREF_JOYSTICK_VALUES_CH6:
 331                     joystickRaw[5] = value;
 332                     break;
 333
 334                 case DREF_JOYSTICK_VALUES_CH7:
 335                     joystickRaw[6] = value;
 336                     break;
 337
 338                 case DREF_JOYSTICK_VALUES_CH8:
 339                     joystickRaw[7] = value;
 340                     break;
 341
 342                 default:
 343                     break;
 344             }
 345         }
 346
 347         if (hpath < 0) {
 348             hpath += 3600;
 349         }
 350
 351         if (yaw < 0){
 352             yaw += 3600;
 353         }
 354
 355         if (hasJoystick) {
 356             uint16_t channelValues[XPLANE_JOYSTICK_AXIS_COUNT];
 357             channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[0]);
 358             channelValues[1] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[1]);
 359             channelValues[2] = FLOAT_0_1_TO_PWM(joystickRaw[2]);
 360             channelValues[3] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[3]);
 361             channelValues[4] = FLOAT_0_1_TO_PWM(joystickRaw[4]);
 362             channelValues[5] = FLOAT_0_1_TO_PWM(joystickRaw[5]);
 363             channelValues[6] = FLOAT_0_1_TO_PWM(joystickRaw[6]);
 364             channelValues[7] = FLOAT_0_1_TO_PWM(joystickRaw[7]);
 365
 366             rxSimSetChannelValue(channelValues, XPLANE_JOYSTICK_AXIS_COUNT);
 367         }
 368
 369         gpsFakeSet(
 370             GPS_FIX_3D,
 371             16,
 372             (int32_t)roundf(lattitude * 10000000),
 373             (int32_t)roundf(longitude * 10000000),
 374             (int32_t)roundf(elevation * 100),
 375             (int16_t)roundf(groundspeed * 100),
 376             (int16_t)roundf(hpath * 10),
 377             0, //(int16_t)roundf(-local_vz * 100),
 378             0, //(int16_t)roundf(local_vx * 100),
 379             0, //(int16_t)roundf(-local_vy * 100),
 380             0
 381         );
 382
 383         const int32_t altitideOverGround = (int32_t)roundf(agl * 100);
 384         if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
 385             fakeRangefindersSetData(altitideOverGround);
 386         } else {
 387             fakeRangefindersSetData(-1);
 388         }
 389
 390         const int16_t roll_inav = roll * 10;
 391         const int16_t pitch_inav = -pitch * 10;
 392         const int16_t yaw_inav = yaw * 10;
 393
 394         if (!useImu) {
 395             imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
 396             imuUpdateAttitude(micros());
 397         }
 398
 399         fakeAccSet(
 400             constrainToInt16(-accel_x * GRAVITY_MSS * 1000.0f),
 401             constrainToInt16(accel_y * GRAVITY_MSS * 1000.0f),
 402             constrainToInt16(accel_z * GRAVITY_MSS * 1000.0f)
 403         );
 404
 405         fakeGyroSet(
 406             constrainToInt16(gyro_x * 16.0f),
 407             constrainToInt16(-gyro_y * 16.0f),
 408             constrainToInt16(-gyro_z * 16.0f)
 409         );
 410
 411         fakeBaroSet((int32_t)roundf(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21));
 412         fakePitotSetAirspeed(airspeed * 100.0f);
 413
 414         fakeBattSensorSetVbat(16.8f * 100);
 415
 416         fpQuaternion_t quat;
 417         fpVector3_t north;
 418         north.x = 1.0f;
 419         north.y = 0.0f;
 420         north.z = 0.0f;
 421         computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
 422         transformVectorEarthToBody(&north, &quat);
 423         fakeMagSet(
 424             constrainToInt16(north.x * 1024.0f),
 425             constrainToInt16(north.y * 1024.0f),
 426             constrainToInt16(north.z * 1024.0f)
 427         );
 428
 429         if (!initalized) {
 430             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
 431             // Aircraft can wobble on the runway and prevents calibration of the accelerometer
 432             ENABLE_STATE(ACCELEROMETER_CALIBRATED);
 433             initalized = true;
 434         }
 435
 436         unlockMainPID();
 437     }
 438
 439     return NULL;
 440 }
 441
 442
 443 bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu)
 444 {
 445     memcpy(pwmMapping, mapping, mapCount);
 446     mappingCount = mapCount;
 447     useImu = imu;
 448
 449     if (port == 0) {
 450         port = XP_PORT; // use default port
 451     }
 452
 453     if(lookupAddress(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) {
 454         return false;
 455     }
 456
 457     sockFd = socket(((struct sockaddr*)&serverAddr)->sa_family, SOCK_DGRAM, IPPROTO_UDP);
 458     if (sockFd < 0) {
 459         return false;
 460     } else {
 461         char addrbuf[IPADDRESS_PRINT_BUFLEN];
 462         char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN );
 463         if (nptr != NULL) {
 464             fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd);
 465         }
 466     }
 467
 468     struct timeval tv;
 469         tv.tv_sec = 1;
 470         tv.tv_usec = 0;
 471         if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
 472         return false;
 473     }
 474
 475     if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
 476         return false;
 477     }
 478
 479     if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
 480         return false;
 481     }
 482
 483     while (!initalized) {
 484         registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
 485         registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
 486         registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
 487         registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100);
 488         registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
 489         registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
 490         registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
 491         registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
 492         registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
 493         registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
 494         registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
 495         registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
 496         registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
 497         registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
 498         registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
 499         registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
 500         registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
 501         registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
 502         registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
 503         registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
 504         registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100);
 505         registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100);
 506         registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100);
 507         registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100);
 508         // Abusing cowl flaps for other channels
 509         registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100);
 510         registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100);
 511         registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100);
 512         registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100);
 513         registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100);
 514         delay(250);
 515     }
 516
 517     return true;
 518 }