src/main/target/SITL/target.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 <stdint.h>
  26 #include <stdlib.h>
  27 #include <stdbool.h>
  28 #include <math.h>
  29 #include <time.h>
  30 #include <string.h>
  31 #include <stdio.h>
  32 #include <getopt.h>
  33 #include <errno.h>
  34 #include <time.h>
  35 #include <pthread.h>
  36 #include <unistd.h>
  37 #include <sys/types.h>
  38 #include <sys/socket.h>
  39 #include <arpa/inet.h>
  40 #include <netdb.h>
  41 #include <netinet/in.h>
  42
  43 #include <platform.h>
  44 #include "target.h"
  45
  46 #include "fc/runtime_config.h"
  47 #include "common/utils.h"
  48 #include "scheduler/scheduler.h"
  49 #include "drivers/system.h"
  50 #include "drivers/pwm_mapping.h"
  51 #include "drivers/timer.h"
  52 #include "drivers/serial.h"
  53 #include "config/config_streamer.h"
  54 #include "build/version.h"
  55
  56 #include "target/SITL/sim/realFlight.h"
  57 #include "target/SITL/sim/xplane.h"
  58
  59 #include "target/SITL/serial_proxy.h"
  60
  61 // More dummys
  62 const int timerHardwareCount = 0;
  63 timerHardware_t timerHardware[1];
  64 uint32_t SystemCoreClock = 500 * 1e6; // fake 500 MHz;
  65 char _estack = 0 ;
  66 char _Min_Stack_Size = 0;
  67
  68 static pthread_mutex_t mainLoopLock;
  69 static SitlSim_e sitlSim = SITL_SIM_NONE;
  70 static struct timespec start_time;
  71 static uint8_t pwmMapping[MAX_MOTORS + MAX_SERVOS];
  72 static uint8_t mappingCount = 0;
  73 static bool useImu = false;
  74 static char *simIp = NULL;
  75 static int simPort = 0;
  76
  77 static char **c_argv;
  78
  79 static void printVersion(void) {
  80     fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision);
  81 }
  82
  83 void systemInit(void) {
  84     printVersion();
  85     clock_gettime(CLOCK_MONOTONIC, &start_time);
  86     fprintf(stderr, "[SYSTEM] Init...\n");
  87
  88 #if !defined(__FreeBSD__)  && !defined(__APPLE__)
  89     pthread_attr_t thAttr;
  90     int policy = 0;
  91
  92     pthread_attr_init(&thAttr);
  93     pthread_attr_getschedpolicy(&thAttr, &policy);
  94     pthread_setschedprio(pthread_self(), sched_get_priority_min(policy));
  95     pthread_attr_destroy(&thAttr);
  96 #endif
  97
  98     if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
  99         fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n");
 100         exit(1);
 101     }
 102
 103     if (sitlSim != SITL_SIM_NONE) {
 104         fprintf(stderr, "[SIM] Waiting for connection...\n");
 105     }
 106
 107     switch (sitlSim) {
 108         case SITL_SIM_REALFLIGHT:
 109             if (mappingCount > RF_MAX_PWM_OUTS) {
 110                 fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
 111                 sitlSim = SITL_SIM_NONE;
 112                 break;
 113             }
 114             if (simRealFlightInit(simIp, pwmMapping, mappingCount, useImu)) {
 115                 fprintf(stderr, "[SIM] Connection with RealFlight successfully established.\n");
 116             } else {
 117                 fprintf(stderr, "[SIM] Connection with RealFlight NOT established.\n");
 118             }
 119             break;
 120         case SITL_SIM_XPLANE:
 121             if (mappingCount > XP_MAX_PWM_OUTS) {
 122                 fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
 123                 sitlSim = SITL_SIM_NONE;
 124                 break;
 125             }
 126             if (simXPlaneInit(simIp, simPort, pwmMapping, mappingCount, useImu)) {
 127                 fprintf(stderr, "[SIM] Connection with X-Plane successfully established.\n");
 128             } else {
 129                 fprintf(stderr, "[SIM] Connection with X-PLane NOT established.\n");
 130             }
 131             break;
 132         default:
 133           fprintf(stderr, "[SIM] No interface specified. Configurator only.\n");
 134           break;
 135     }
 136
 137     rescheduleTask(TASK_SERIAL, SITL_SERIAL_TASK_US);
 138 }
 139
 140 bool parseMapping(char* mapStr)
 141 {
 142     char *split = strtok(mapStr, ",");
 143     char numBuf[2];
 144     while(split)
 145     {
 146         if (strlen(split) != 6) {
 147             return false;
 148         }
 149
 150         if (split[0] == 'M' || split[0] == 'S') {
 151             memcpy(numBuf, &split[1], 2);
 152             int pwmOut = atoi(numBuf);
 153             memcpy(numBuf, &split[4], 2);
 154             int rOut = atoi(numBuf);
 155             if (pwmOut < 0 || rOut < 1) {
 156                 return false;
 157             }
 158             if (split[0] == 'M') {
 159                 pwmMapping[rOut - 1] = pwmOut - 1;
 160                 pwmMapping[rOut - 1] |= 0x80;
 161                 mappingCount++;
 162             } else if (split[0] == 'S') {
 163                 pwmMapping[rOut - 1] = pwmOut;
 164                 mappingCount++;
 165             }
 166         } else {
 167             return false;
 168         }
 169         split = strtok(NULL, ",");
 170     }
 171
 172     return true;
 173 }
 174
 175 OptSerialStopBits_e parseStopBits(const char* optarg){
 176     if ( strcmp(optarg, "One") == 0 ) {
 177         return OPT_SERIAL_STOP_BITS_ONE;
 178     } else if ( strcmp(optarg, "Two") == 0 ) {
 179         return OPT_SERIAL_STOP_BITS_TWO;
 180     } else  {
 181         return OPT_SERIAL_STOP_BITS_INVALID;
 182     }
 183 }
 184
 185 OptSerialParity_e parseParity(const char* optarg){
 186     if ( strcmp(optarg, "Even") == 0 ) {
 187         return OPT_SERIAL_PARITY_EVEN;
 188     } else if ( strcmp(optarg, "None") == 0 ) {
 189         return OPT_SERIAL_PARITY_NONE;
 190     } else if ( strcmp(optarg, "Odd") == 0 ) {
 191         return OPT_SERIAL_PARITY_ODD;
 192     } else  {
 193         return OPT_SERIAL_PARITY_INVALID;
 194     }
 195 }
 196
 197 void printCmdLineOptions(void)
 198 {
 199     printVersion();
 200     fprintf(stderr, "Avaiable options:\n");
 201     fprintf(stderr, "--path=[path]                  Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
 202     fprintf(stderr, "--sim=[rf|xp]                  Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
 203     fprintf(stderr, "--simip=[ip]                   IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
 204     fprintf(stderr, "--simport=[port]               Port oft the simulator host.\n");
 205     fprintf(stderr, "--useimu                       Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
 206     fprintf(stderr, "--serialuart=[uart]            UART number on which serial receiver is configured in SITL, f.e. 3 for UART3\n");
 207     fprintf(stderr, "--serialport=[serialport]      Host's serial port to which serial receiver/proxy FC is connected, f.e. COM3, /dev/ttyACM3\n");
 208     fprintf(stderr, "--baudrate=[baudrate]          Serial receiver baudrate (default: 115200).\n");
 209     fprintf(stderr, "--stopbits=[None|One|Two]      Serial receiver stopbits (default: One).\n");
 210     fprintf(stderr, "--parity=[Even|None|Odd]       Serial receiver parity (default: None).\n");
 211     fprintf(stderr, "--fcproxy                      Use inav/betaflight FC as a proxy for serial receiver.\n");
 212     fprintf(stderr, "--chanmap=[mapstring]          Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
 213     fprintf(stderr, "                               The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n");
 214     fprintf(stderr, "                               For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
 215     fprintf(stderr, "                               --chanmap=M01-01,S01-02,S02-03\n");
 216 }
 217
 218 void parseArguments(int argc, char *argv[])
 219 {
 220     // Stash these so we can rexec on reboot, just like a FC does
 221     c_argv = calloc(argc+1, sizeof(char *));
 222     for (int i = 0; i < argc; i++) {
 223         c_argv[i] = strdup(argv[i]);
 224     }
 225     int c;
 226     while(true) {
 227         static struct option longOpt[] = {
 228             {"sim", required_argument, 0, 's'},
 229             {"useimu", no_argument, 0, 'u'},
 230             {"chanmap", required_argument, 0, 'c'},
 231             {"simip", required_argument, 0, 'i'},
 232             {"simport", required_argument, 0, 'p'},
 233             {"help", no_argument, 0, 'h'},
 234             {"path", required_argument, 0, 'e'},
 235             {"version", no_argument, 0, 'v'},
 236             {"serialuart", required_argument, 0, '0'},
 237             {"serialport", required_argument, 0, '1'},
 238             {"baudrate", required_argument, 0, '2'},
 239             {"stopbits", required_argument, 0, '3'},
 240             {"parity", required_argument, 0, '4'},
 241             {"fcproxy", no_argument, 0, '5'},
 242             {NULL, 0, NULL, 0}
 243         };
 244
 245         c = getopt_long_only(argc, argv, "s:c:h", longOpt, NULL);
 246         if (c == -1)
 247             break;
 248
 249         switch (c) {
 250             case 's':
 251                 if (strcmp(optarg, "rf") == 0) {
 252                     sitlSim = SITL_SIM_REALFLIGHT;
 253                 } else if (strcmp(optarg, "xp") == 0){
 254                     sitlSim = SITL_SIM_XPLANE;
 255                 } else {
 256                     fprintf(stderr, "[SIM] Unsupported simulator %s.\n", optarg);
 257                 }
 258                 break;
 259
 260             case 'c':
 261                 if (!parseMapping(optarg) && sitlSim != SITL_SIM_NONE) {
 262                     fprintf(stderr, "[SIM] Invalid channel mapping string.\n");
 263                     printCmdLineOptions();
 264                     exit(0);
 265                 }
 266                 break;
 267             case 'p':
 268                 simPort = atoi(optarg);
 269                 break;
 270             case 'u':
 271                 useImu = true;
 272                 break;
 273             case 'i':
 274                 simIp = optarg;
 275                 break;
 276             case 'e':
 277                 if (!configFileSetPath(optarg)){
 278                     fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
 279                 }
 280                 break;
 281             case 'v':
 282                 printVersion();
 283                 exit(0);
 284             case '0':
 285                 serialUartIndex = atoi(optarg);
 286                 if ( (serialUartIndex<1) || (serialUartIndex>8) ) {
 287                     fprintf(stderr, "[serialuart] Invalid argument\n.");
 288                     exit(0);
 289                 }
 290                 break;
 291             case '1':
 292                 if ( (strlen(optarg)<1) || (strlen(optarg)>63) ) {
 293                     fprintf(stderr, "[serialport] Invalid argument\n.");
 294                     exit(0);
 295                 } else {
 296                     strcpy( serialPort, optarg );
 297                 }
 298                 break;
 299             case '2':
 300                 serialBaudRate = atoi(optarg);
 301                 if ( serialBaudRate < 1200 )
 302                 {
 303                     fprintf(stderr, "[baudrate] Invalid argument\n.");
 304                     exit(0);
 305                 }
 306                 break;
 307             case '3':
 308                 serialStopBits = parseStopBits(optarg);
 309                 if ( serialStopBits == OPT_SERIAL_STOP_BITS_INVALID )
 310                 {
 311                     fprintf(stderr, "[stopbits] Invalid argument\n.");
 312                     exit(0);
 313                 }
 314                 break;
 315
 316             case '4':
 317                 serialParity = parseParity(optarg);
 318                 if ( serialParity== OPT_SERIAL_PARITY_INVALID )
 319                 {
 320                     fprintf(stderr, "[parity] Invalid argument\n.");
 321                     exit(0);
 322                 }
 323                 break;
 324             case '5':
 325                 serialFCProxy = true;
 326                 break;
 327
 328             default:
 329                 printCmdLineOptions();
 330                 exit(0);
 331         }
 332     }
 333
 334     if (simIp == NULL) {
 335         simIp = malloc(10);
 336         strcpy(simIp, "127.0.0.1");
 337     }
 338 }
 339
 340
 341 bool lockMainPID(void) {
 342     return pthread_mutex_trylock(&mainLoopLock) == 0;
 343 }
 344
 345 void unlockMainPID(void)
 346 {
 347     pthread_mutex_unlock(&mainLoopLock);
 348 }
 349
 350 // Replacements for system functions
 351 timeUs_t micros(void) {
 352     struct timespec now;
 353     clock_gettime(CLOCK_MONOTONIC, &now);
 354
 355     return (now.tv_sec - start_time.tv_sec) * 1000000 + (now.tv_nsec - start_time.tv_nsec) / 1000;
 356 }
 357
 358 uint64_t microsISR(void)
 359 {
 360     return micros();
 361 }
 362
 363 uint32_t millis(void) {
 364     return (uint32_t)(micros() / 1000);
 365 }
 366
 367 void delayMicroseconds(timeUs_t us)
 368 {
 369     usleep(us);
 370 }
 371
 372 void delay(timeMs_t ms)
 373 {
 374     delayMicroseconds(ms * 1000UL);
 375 }
 376
 377 void systemReset(void)
 378 {
 379     fprintf(stderr, "[SYSTEM] Reset\n");
 380 #if defined(__CYGWIN__) || defined(__APPLE__) || GCC_MAJOR < 12
 381     for(int j = 3; j < 1024; j++) {
 382         close(j);
 383     }
 384 #else
 385     closefrom(3);
 386 #endif
 387     serialProxyClose();
 388     execvp(c_argv[0], c_argv); // restart
 389 }
 390
 391 void systemResetToBootloader(void)
 392 {
 393     fprintf(stderr, "[SYSTEM] Reset to bootloader\n");
 394     exit(0);
 395 }
 396
 397 void failureMode(failureMode_e mode) {
 398     fprintf(stderr, "[SYSTEM] Failure mode %d\n", mode);
 399     while (true) {
 400         delay(1000);
 401     };
 402 }
 403
 404 // Even more dummys and stubs
 405 uint32_t getEscUpdateFrequency(void)
 406 {
 407     return 400;
 408 }
 409
 410 pwmInitError_e getPwmInitError(void)
 411 {
 412     return PWM_INIT_ERROR_NONE;
 413 }
 414
 415 const char *getPwmInitErrorMessage(void)
 416 {
 417     return "No error";
 418 }
 419
 420 void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 421 {
 422     UNUSED(io);
 423     UNUSED(cfg);
 424 }
 425
 426 void timerInit(void) {
 427     // NOP
 428 }
 429
 430 bool isMPUSoftReset(void)
 431 {
 432     return false;
 433 }
 434
 435 // Not in linux libs, but in arm-none-eabi ?!?
 436 // https://github.com/lattera/freebsd/blob/master/lib/libc/string/strnstr.c
 437 char * strnstr(const char *s, const char *find, size_t slen)
 438 {
 439         char c, sc;
 440         size_t len;
 441
 442         if ((c = *find++) != '\0') {
 443                 len = strlen(find);
 444                 do {
 445                         do {
 446                                 if (slen-- < 1 || (sc = *s++) == '\0')
 447                                         return (NULL);
 448                         } while (sc != c);
 449                         if (len > slen)
 450                                 return (NULL);
 451                 } while (strncmp(s, find, len) != 0);
 452                 s--;
 453         }
 454         return ((char *)s);
 455 }
 456
 457 int lookupAddress (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
 458 {
 459     struct addrinfo *servinfo, *p;
 460     struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG};
 461     if (name == NULL) {
 462         hints.ai_flags |= AI_PASSIVE;
 463     }
 464   /*
 465     This nonsense is to uniformly deliver the same sa_family regardless of whether
 466     name is NULL or non-NULL ** ON LINUX **
 467     Otherwise, at least on Linux, we get
 468     - V6,V4 for the non-null case and
 469     - V4,V6 for the null case, regardless of gai.conf
 470     Which may confuse consumers
 471     FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks
 472     unless a quad dotted address is specified (or a name resolveds to V4,
 473     or system policy enforces IPv4 over V6
 474   */
 475     struct addrinfo *p4 = NULL;
 476     struct addrinfo *p6 = NULL;
 477
 478     int result;
 479     char aport[16];
 480     snprintf(aport, sizeof(aport), "%d", port);
 481
 482     if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) {
 483         fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
 484         return result;
 485     } else {
 486         for(p = servinfo; p != NULL; p = p->ai_next) {
 487             if(p->ai_family == AF_INET6)
 488                 p6 = p;
 489             else if(p->ai_family == AF_INET)
 490                 p4 = p;
 491         }
 492
 493         if (p6 != NULL)
 494             p = p6;
 495         else if (p4 != NULL)
 496             p = p4;
 497         else
 498             return -1;
 499         memcpy(addr, p->ai_addr, p->ai_addrlen);
 500         *len = p->ai_addrlen;
 501         freeaddrinfo(servinfo);
 502     }
 503     return 0;
 504 }
 505
 506 char *prettyPrintAddress(struct sockaddr* p, char *outbuf, size_t buflen)
 507 {
 508     if (buflen < IPADDRESS_PRINT_BUFLEN) {
 509         return NULL;
 510     }
 511     char *bufp = outbuf;
 512     void *addr;
 513     uint16_t port;
 514     if (p->sa_family == AF_INET6) {
 515         struct sockaddr_in6 * ip = (struct sockaddr_in6*)p;
 516         addr = &ip->sin6_addr;
 517         port = ntohs(ip->sin6_port);
 518     } else {
 519         struct sockaddr_in * ip = (struct sockaddr_in*)p;
 520         port = ntohs(ip->sin_port);
 521         addr = &ip->sin_addr;
 522     }
 523     const char *res = inet_ntop(p->sa_family, addr, outbuf+1, buflen-1);
 524     if (res != NULL) {
 525         char *ptr = (char*)res+strlen(res);
 526         if (p->sa_family == AF_INET6) {
 527             *bufp ='[';
 528             *ptr++ = ']';
 529         } else {
 530             bufp++;
 531         }
 532         sprintf(ptr, ":%d", port);
 533         return bufp;
 534     }
 535     return NULL;
 536 }