src/main/target/SITL/sim/realFlight.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 <pthread.h>
  31 #include <math.h>
  32
  33 #include "platform.h"
  34
  35 #include "target.h"
  36 #include "target/SITL/sim/realFlight.h"
  37 #include "target/SITL/sim/simple_soap_client.h"
  38 #include "target/SITL/sim/xplane.h"
  39 #include "target/SITL/sim/simHelper.h"
  40 #include "fc/runtime_config.h"
  41 #include "drivers/time.h"
  42 #include "drivers/accgyro/accgyro_fake.h"
  43 #include "drivers/barometer/barometer_fake.h"
  44 #include "sensors/battery_sensor_fake.h"
  45 #include "sensors/acceleration.h"
  46 #include "sensors/barometer.h"
  47 #include "drivers/pitotmeter/pitotmeter_fake.h"
  48 #include "drivers/compass/compass_fake.h"
  49 #include "drivers/rangefinder/rangefinder_virtual.h"
  50 #include "io/rangefinder.h"
  51 #include "common/utils.h"
  52 #include "common/maths.h"
  53 #include "flight/mixer.h"
  54 #include "flight/servos.h"
  55 #include "flight/imu.h"
  56 #include "io/gps.h"
  57 #include "rx/sim.h"
  58
  59 #define RF_PORT 18083
  60 #define RF_MAX_CHANNEL_COUNT 12
  61
  62 // "RealFlight Ranch" is located in Sierra Nevada, southern Spain
  63 // This is not the Position of the Ranch, it's the Point of 0,0 in the Map (bottom left corner)
  64 #define FAKE_LAT  36.910610f
  65 #define FAKE_LON  -2.876605f
  66
  67 static uint8_t pwmMapping[RF_MAX_PWM_OUTS];
  68 static uint8_t mappingCount;
  69
  70 static pthread_cond_t sockcond1 = PTHREAD_COND_INITIALIZER;
  71 static pthread_cond_t sockcond2 = PTHREAD_COND_INITIALIZER;
  72 static pthread_mutex_t sockmtx = PTHREAD_MUTEX_INITIALIZER;
  73
  74 static soap_client_t *client = NULL;
  75 static soap_client_t *clientNext = NULL;
  76
  77 static pthread_t soapThread;
  78 static pthread_t creationThread;
  79
  80 static bool isInitalised = false;
  81 static bool useImu = false;
  82
  83 typedef struct
  84 {
  85     float m_channelValues[RF_MAX_PWM_OUTS];
  86     float m_currentPhysicsSpeedMultiplier;
  87     float m_currentPhysicsTime_SEC;
  88     float m_airspeed_MPS;
  89     float m_altitudeASL_MTR;
  90     float m_altitudeAGL_MTR;
  91     float m_groundspeed_MPS;
  92     float m_pitchRate_DEGpSEC;
  93     float m_rollRate_DEGpSEC;
  94     float m_yawRate_DEGpSEC;
  95     float m_azimuth_DEG;
  96     float m_inclination_DEG;
  97     float m_roll_DEG;
  98     float m_orientationQuaternion_X;
  99     float m_orientationQuaternion_Y;
 100     float m_orientationQuaternion_Z;
 101     float m_orientationQuaternion_W;
 102     float m_aircraftPositionX_MTR;
 103     float m_aircraftPositionY_MTR;
 104     float m_velocityWorldU_MPS;
 105     float m_velocityWorldV_MPS;
 106     float m_velocityWorldW_MPS;
 107     float m_velocityBodyU_MPS;
 108     float m_velocityBodyV_MPS;
 109     float m_velocityBodyW_MPS;
 110     float m_accelerationWorldAX_MPS2;
 111     float m_accelerationWorldAY_MPS2;
 112     float m_accelerationWorldAZ_MPS2;
 113     float m_accelerationBodyAX_MPS2;
 114     float m_accelerationBodyAY_MPS2;
 115     float m_accelerationBodyAZ_MPS2;
 116     float m_windX_MPS;
 117     float m_windY_MPS;
 118     float m_windZ_MPSPS;
 119     float m_propRPM;
 120     float m_heliMainRotorRPM;
 121     float m_batteryVoltage_VOLTS;
 122     float m_batteryCurrentDraw_AMPS;
 123     float m_batteryRemainingCapacity_MAH;
 124     float m_fuelRemaining_OZ;
 125     bool m_isLocked;
 126     bool m_hasLostComponents;
 127     bool m_anEngineIsRunning;
 128     bool m_isTouchingGround;
 129     bool m_flightAxisControllerIsActive;
 130     char* m_currentAircraftStatus;
 131     bool m_resetButtonHasBeenPressed;
 132 } rfValues_t;
 133
 134 rfValues_t rfValues;
 135
 136 static void deleteClient(soap_client_t *client)
 137 {
 138     soapClientClose(client);
 139     free(client);
 140     client = NULL;
 141 }
 142
 143 static void startRequest(char* action, const char* fmt, ...)
 144 {
 145     pthread_mutex_lock(&sockmtx);
 146     while (clientNext == NULL) {
 147         pthread_cond_wait(&sockcond1, &sockmtx);
 148     }
 149
 150     client = clientNext;
 151     clientNext = NULL;
 152
 153     pthread_cond_broadcast(&sockcond2);
 154     pthread_mutex_unlock(&sockmtx);
 155
 156     va_list va;
 157     va_start(va, fmt);
 158     soapClientSendRequestVa(client, action, fmt, va);
 159     va_end(va);
 160 }
 161
 162 static char* endRequest(void)
 163 {
 164    char* ret = soapClientReceive(client);
 165    deleteClient(client);
 166    return ret;
 167 }
 168
 169 // Simple, but fast ;)
 170 static double getDoubleFromResponse(const char* response, const char* elementName)
 171 {
 172     if (!response) {
 173         return 0;
 174     }
 175
 176     char* pos = strstr(response, elementName);
 177     if (!pos) {
 178         return 0;
 179     }
 180     return atof(pos + strlen(elementName) + 1);
 181 }
 182
 183
 184 /*
 185 Currently unused
 186 static bool getBoolFromResponse(const char* response, const char* elementName)
 187 {
 188     if (!response) {
 189         return false;
 190     }
 191
 192     char* pos = strstr(response, elementName);
 193     if (!pos) {
 194         return false;
 195     }
 196     return (strncmp(pos + strlen(elementName) + 1, "true", 4) == 0);
 197 }
 198 */
 199
 200 static char* getStringFromResponse(const char* response, const char* elementName)
 201 {
 202     if (!response) {
 203         return 0;
 204     }
 205
 206     char* pos = strstr(response, elementName);
 207     if (!pos) {
 208         return NULL;
 209     }
 210
 211     pos += strlen(elementName) + 1;
 212     char* end = strstr(pos, "</");
 213     if (!end) {
 214         return NULL;
 215     }
 216     size_t length = end - pos;
 217     char* ret = calloc(length + 1, sizeof(char));
 218     strncpy(ret, pos, length);
 219     return ret;
 220 }
 221
 222 static bool getChannelValues(const char* response, uint16_t* channelValues)
 223 {
 224     if (!response) {
 225         return false;
 226     }
 227
 228     const char* channelValueTag = "<m-channelValues-0to1 xsi:type=\"SOAP-ENC:Array\" SOAP-ENC:arrayType=\"xsd:double[12]\">";
 229     char* pos = strstr(response, channelValueTag);
 230     if (!pos){
 231         return false;
 232     }
 233
 234     pos += strlen(channelValueTag);
 235     for (size_t i = 0; i < RF_MAX_CHANNEL_COUNT; i++) {
 236         char* end = strstr(pos, "</");
 237         if (!end) {
 238             return false;
 239         }
 240
 241         channelValues[i] = FLOAT_0_1_TO_PWM((float)atof(pos + 6));
 242         pos = end + 7;
 243     }
 244
 245     return true;
 246 }
 247
 248
 249 static void fakeCoords(float posX, float posY, float distanceX, float distanceY, float *lat, float *lon)
 250 {
 251     float m = 1 / (2 * M_PIf / 360 * EARTH_RADIUS) / 1000;
 252     *lat = (posX + (distanceX * m));
 253     *lon = (posY + (distanceY * m) / cosf(posX * (M_PIf / 180)));
 254 }
 255
 256 static float convertAzimuth(float azimuth)
 257 {
 258     if (azimuth < 0) {
 259         azimuth += 360;
 260     }
 261     return 360 - fmodf(azimuth + 90, 360.0f);
 262 }
 263
 264 static void exchangeData(void)
 265 {
 266     double servoValues[RF_MAX_PWM_OUTS] = {  };
 267     for (int i = 0; i < mappingCount; i++) {
 268         if (pwmMapping[i] & 0x80) { // Motor
 269             servoValues[i] = (double)PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]);
 270         } else {
 271             servoValues[i] = (double)PWM_TO_FLOAT_0_1(servo[pwmMapping[i]]);
 272         }
 273     }
 274
 275     startRequest("ExchangeData", "<ExchangeData><pControlInputs><m-selectedChannels>%u</m-selectedChannels>"
 276         "<m-channelValues-0to1><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item>"
 277         "<item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item></m-channelValues-0to1></pControlInputs></ExchangeData>",
 278         0xFFF,
 279         servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7],
 280         servoValues[8], servoValues[9], servoValues[10], servoValues[11]);
 281     char* response = endRequest();
 282
 283     //rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC");
 284     //rfValues.m_currentPhysicsSpeedMultiplier = getDoubleFromResponse(response, "m-currentPhysicsSpeedMultiplier");
 285     rfValues.m_airspeed_MPS = getDoubleFromResponse(response, "m-airspeed-MPS");
 286     rfValues.m_altitudeASL_MTR = getDoubleFromResponse(response, "m-altitudeASL-MTR");
 287     rfValues.m_altitudeAGL_MTR = getDoubleFromResponse(response, "m-altitudeAGL-MTR");
 288     rfValues.m_groundspeed_MPS = getDoubleFromResponse(response, "m-groundspeed-MPS");
 289     rfValues.m_pitchRate_DEGpSEC = getDoubleFromResponse(response, "m-pitchRate-DEGpSEC");
 290     rfValues.m_rollRate_DEGpSEC = getDoubleFromResponse(response, "m-rollRate-DEGpSEC");
 291     rfValues.m_yawRate_DEGpSEC = getDoubleFromResponse(response, "m-yawRate-DEGpSEC");
 292     rfValues.m_azimuth_DEG = getDoubleFromResponse(response, "m-azimuth-DEG");
 293     rfValues.m_inclination_DEG = getDoubleFromResponse(response, "m-inclination-DEG");
 294     rfValues.m_roll_DEG = getDoubleFromResponse(response, "m-roll-DEG");
 295     //rfValues.m_orientationQuaternion_X = getDoubleFromResponse(response, "m-orientationQuaternion-X");
 296     //rfValues.m_orientationQuaternion_Y = getDoubleFromResponse(response, "m-orientationQuaternion-Y");
 297     //rfValues.m_orientationQuaternion_Z = getDoubleFromResponse(response, "m-orientationQuaternion-Z");
 298     //rfValues.m_orientationQuaternion_W = getDoubleFromResponse(response, "m-orientationQuaternion-W");
 299     rfValues.m_aircraftPositionX_MTR = getDoubleFromResponse(response, "m-aircraftPositionX-MTR");
 300     rfValues.m_aircraftPositionY_MTR = getDoubleFromResponse(response, "m-aircraftPositionY-MTR");
 301     rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
 302     rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
 303     rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
 304     //rfValues.m_velocityBodyU_MPS = getDoubleFromResponse(response, "m-velocityBodyU-MPS");
 305     //rfValues.m_velocityBodyV_MPS = getDoubleFromResponse(response, "mm-velocityBodyV-MPS");
 306     //rfValues.m_velocityBodyW_MPS = getDoubleFromResponse(response, "m-velocityBodyW-MPS");
 307     //rfValues.m_accelerationWorldAX_MPS2 = getDoubleFromResponse(response, "m-accelerationWorldAX-MPS2");
 308     //rfValues.m_accelerationWorldAY_MPS2 = getDoubleFromResponse(response, "m-accelerationWorldAY-MPS2");
 309     //rfValues.m_accelerationWorldAZ_MPS2 = getDoubleFromResponse(response, "m-accelerationWorldAZ-MPS2");
 310     rfValues.m_accelerationBodyAX_MPS2 = getDoubleFromResponse(response, "m-accelerationBodyAX-MPS2");
 311     rfValues.m_accelerationBodyAY_MPS2 = getDoubleFromResponse(response, "m-accelerationBodyAY-MPS2");
 312     rfValues.m_accelerationBodyAZ_MPS2 = getDoubleFromResponse(response, "m-accelerationBodyAZ-MPS2");
 313     //rfValues.m_windX_MPS = getDoubleFromResponse(response, "m-windX-MPS");
 314     //rfValues.m_windY_MPS = getDoubleFromResponse(response, "m-windY-MPS");
 315     //rfValues.m_windZ_MPSPS = getDoubleFromResponse(response, "m-windZ-MPS");
 316     //rfValues.m_propRPM = getDoubleFromResponse(response, "m-propRPM");
 317     //rfValues.m_heliMainRotorRPM = getDoubleFromResponse(response, "m-heliMainRotorRPM");
 318     rfValues.m_batteryVoltage_VOLTS = getDoubleFromResponse(response, "m-batteryVoltage-VOLTS");
 319     rfValues.m_batteryCurrentDraw_AMPS = getDoubleFromResponse(response, "m-batteryCurrentDraw-AMPS");
 320     //rfValues.m_batteryRemainingCapacity_MAH = getDoubleFromResponse(response, "m-batteryRemainingCapacity-MAH");
 321     //rfValues.m_fuelRemaining_OZ = getDoubleFromResponse(response, "m-fuelRemaining-OZ");
 322     //rfValues.m_isLocked = getBoolFromResponse(response, "m-isLocked");
 323     //rfValues.m_hasLostComponents = getBoolFromResponse(response, "m-hasLostComponents");
 324     //rfValues.m_anEngineIsRunning = getBoolFromResponse(response, "m-anEngineIsRunning");
 325     //rfValues.m_isTouchingGround = getBoolFromResponse(response, "m-isTouchingGround");
 326     //rfValues.m_flightAxisControllerIsActive= getBoolFromResponse(response, "m-flightAxisControllerIsActive");
 327     rfValues.m_currentAircraftStatus = getStringFromResponse(response, "m-currentAircraftStatus");
 328
 329
 330     uint16_t channelValues[RF_MAX_CHANNEL_COUNT];
 331     getChannelValues(response, channelValues);
 332     rxSimSetChannelValue(channelValues, RF_MAX_CHANNEL_COUNT);
 333
 334     float lat, lon;
 335     fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon);
 336
 337     int16_t course = (int16_t)roundf(RADIANS_TO_DECIDEGREES(atan2_approx(-rfValues.m_velocityWorldU_MPS,rfValues.m_velocityWorldV_MPS)));
 338     int32_t altitude = (int32_t)roundf(rfValues.m_altitudeASL_MTR * 100);
 339     gpsFakeSet(
 340         GPS_FIX_3D,
 341         16,
 342         (int32_t)roundf(lat * 10000000),
 343         (int32_t)roundf(lon * 10000000),
 344         altitude,
 345         (int16_t)roundf(rfValues.m_groundspeed_MPS * 100),
 346         course,
 347         0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction
 348         0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),
 349         0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),
 350         0
 351     );
 352
 353     int32_t altitudeOverGround = (int32_t)roundf(rfValues.m_altitudeAGL_MTR * 100);
 354     if (altitudeOverGround > 0 && altitudeOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
 355         fakeRangefindersSetData(altitudeOverGround);
 356     } else {
 357         fakeRangefindersSetData(-1);
 358     }
 359
 360     const int16_t roll_inav = (int16_t)roundf(rfValues.m_roll_DEG * 10);
 361     const int16_t pitch_inav = (int16_t)roundf(-rfValues.m_inclination_DEG * 10);
 362     const int16_t yaw_inav = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
 363     if (!useImu) {
 364         imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
 365         imuUpdateAttitude(micros());
 366     }
 367
 368     // RealFlights acc data is weird if the aircraft has not yet taken off. Fake 1G in horizontale position
 369     int16_t accX = 0;
 370     int16_t accY = 0;
 371     int16_t accZ = 0;
 372     if (rfValues.m_currentAircraftStatus && strncmp(rfValues.m_currentAircraftStatus, "CAS-WAITINGTOLAUNCH", strlen(rfValues.m_currentAircraftStatus)) == 0) {
 373         accX = 0;
 374         accY = 0;
 375         accZ = (int16_t)(GRAVITY_MSS * 1000.0f);
 376     } else {
 377          accX = constrainToInt16(rfValues.m_accelerationBodyAX_MPS2 * 1000);
 378          accY = constrainToInt16(-rfValues.m_accelerationBodyAY_MPS2 * 1000);
 379          accZ = constrainToInt16(-rfValues.m_accelerationBodyAZ_MPS2 * 1000);
 380     }
 381
 382     fakeAccSet(accX, accY, accZ);
 383
 384     fakeGyroSet(
 385         constrainToInt16(rfValues.m_rollRate_DEGpSEC * 16.0f),
 386         constrainToInt16(-rfValues.m_pitchRate_DEGpSEC * 16.0f),
 387         constrainToInt16(rfValues.m_yawRate_DEGpSEC * 16.0f)
 388     );
 389
 390     fakeBaroSet(altitudeToPressure(altitude), DEGREES_TO_CENTIDEGREES(21));
 391     fakePitotSetAirspeed(rfValues.m_airspeed_MPS * 100);
 392
 393     fakeBattSensorSetVbat((uint16_t)roundf(rfValues.m_batteryVoltage_VOLTS * 100));
 394     fakeBattSensorSetAmperage((uint16_t)roundf(rfValues.m_batteryCurrentDraw_AMPS * 100));
 395
 396     fpQuaternion_t quat;
 397     fpVector3_t north;
 398     north.x = 1.0f;
 399     north.y = 0;
 400     north.z = 0;
 401     computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
 402     transformVectorEarthToBody(&north, &quat);
 403     fakeMagSet(
 404         constrainToInt16(north.x * 16000.0f),
 405         constrainToInt16(north.y * 16000.0f),
 406         constrainToInt16(north.z * 16000.0f)
 407     );
 408
 409     free(rfValues.m_currentAircraftStatus);
 410     free(response);
 411 }
 412
 413 static void* soapWorker(void* arg)
 414 {
 415     UNUSED(arg);
 416     while(true)
 417     {
 418         if (!isInitalised) {
 419             startRequest("RestoreOriginalControllerDevice", "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>");
 420             free(endRequest());
 421             startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
 422             free(endRequest());
 423             exchangeData();
 424             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
 425
 426             isInitalised = true;
 427         }
 428
 429         exchangeData();
 430         unlockMainPID();
 431     }
 432
 433     return NULL;
 434 }
 435
 436
 437 static void* creationWorker(void* arg)
 438 {
 439     char* ip = (char*)arg;
 440
 441     while (true) {
 442         pthread_mutex_lock(&sockmtx);
 443         while (clientNext != NULL) {
 444             pthread_cond_wait(&sockcond2, &sockmtx);
 445         }
 446         pthread_mutex_unlock(&sockmtx);
 447
 448         soap_client_t *cli = malloc(sizeof(soap_client_t));
 449         if (!soapClientConnect(cli, ip, RF_PORT)) {
 450             continue;
 451         }
 452
 453         clientNext = cli;
 454         pthread_mutex_lock(&sockmtx);
 455         pthread_cond_broadcast(&sockcond1);
 456         pthread_mutex_unlock(&sockmtx);
 457     }
 458
 459     return NULL;
 460 }
 461
 462 bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu)
 463 {
 464     memcpy(pwmMapping, mapping, mapCount);
 465     mappingCount = mapCount;
 466     useImu = imu;
 467
 468     if (pthread_create(&soapThread, NULL, soapWorker, NULL) < 0) {
 469         return false;
 470     }
 471
 472     if (pthread_create(&creationThread, NULL, creationWorker, (void*)ip) < 0) {
 473         return false;
 474     }
 475
 476     // Wait until the connection is established, the interface has been initialised
 477     // and the first valid packet has been received to avoid problems with the startup calibration.
 478     while (!isInitalised) {
 479         delay(250);
 480     }
 481
 482     return true;
 483 }