src/platform/SIMULATOR/sitl.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <math.h>
  26 #include <string.h>
  27
  28 #include <errno.h>
  29 #include <time.h>
  30
  31 #include "common/maths.h"
  32
  33 #include "build/debug.h"
  34
  35 #include "drivers/adc_impl.h"
  36 #include "drivers/io.h"
  37 #include "drivers/dma.h"
  38 #include "drivers/motor_impl.h"
  39 #include "drivers/serial.h"
  40 #include "drivers/serial_tcp.h"
  41 #include "drivers/system.h"
  42 #include "drivers/time.h"
  43 #include "drivers/pwm_output.h"
  44 #include "drivers/light_led.h"
  45
  46 #include "drivers/timer.h"
  47 #include "timer_def.h"
  48
  49 #include "drivers/accgyro/accgyro_virtual.h"
  50 #include "drivers/barometer/barometer_virtual.h"
  51 #include "flight/imu.h"
  52
  53 #include "config/feature.h"
  54 #include "config/config.h"
  55 #include "config/config_streamer.h"
  56 #include "config/config_streamer_impl.h"
  57 #include "config/config_eeprom_impl.h"
  58
  59 #include "scheduler/scheduler.h"
  60
  61 #include "pg/rx.h"
  62 #include "pg/motor.h"
  63
  64 #include "rx/rx.h"
  65 #include "rx/spektrum.h"
  66
  67 #include "io/gps.h"
  68 #include "io/gps_virtual.h"
  69
  70 #include "dyad.h"
  71 #include "udplink.h"
  72
  73 uint32_t SystemCoreClock;
  74
  75 static fdm_packet fdmPkt;
  76 static rc_packet rcPkt;
  77 static servo_packet pwmPkt;
  78 static servo_packet_raw pwmRawPkt;
  79
  80 static bool rc_received = false;
  81 static bool fdm_received = false;
  82
  83 static struct timespec start_time;
  84 static double simRate = 1.0;
  85 static pthread_t tcpWorker, udpWorker, udpWorkerRC;
  86 static bool workerRunning = true;
  87 static udpLink_t stateLink, pwmLink, pwmRawLink, rcLink;
  88 static pthread_mutex_t updateLock;
  89 static pthread_mutex_t mainLoopLock;
  90 static char simulator_ip[32] = "127.0.0.1";
  91
  92 #define PORT_PWM_RAW    9001    // Out
  93 #define PORT_PWM        9002    // Out
  94 #define PORT_STATE      9003    // In
  95 #define PORT_RC         9004    // In
  96
  97 int targetParseArgs(int argc, char * argv[])
  98 {
  99     //The first argument should be target IP.
 100     if (argc > 1) {
 101         strcpy(simulator_ip, argv[1]);
 102     }
 103
 104     printf("[SITL] The SITL will output to IP %s:%d (Gazebo) and %s:%d (RealFlightBridge)\n",
 105            simulator_ip, PORT_PWM, simulator_ip, PORT_PWM_RAW);
 106     return 0;
 107 }
 108
 109 int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y);
 110
 111 int lockMainPID(void)
 112 {
 113     return pthread_mutex_trylock(&mainLoopLock);
 114 }
 115
 116 #define RAD2DEG (180.0 / M_PI)
 117 #define ACC_SCALE (256 / 9.80665)
 118 #define GYRO_SCALE (16.4)
 119
 120 static void sendMotorUpdate(void)
 121 {
 122     udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
 123 }
 124
 125 static void updateState(const fdm_packet* pkt)
 126 {
 127     static double last_timestamp = 0; // in seconds
 128     static uint64_t last_realtime = 0; // in uS
 129     static struct timespec last_ts; // last packet
 130
 131     struct timespec now_ts;
 132     clock_gettime(CLOCK_MONOTONIC, &now_ts);
 133
 134     const uint64_t realtime_now = micros64_real();
 135     if (realtime_now > last_realtime + 500*1e3) { // 500ms timeout
 136         last_timestamp = pkt->timestamp;
 137         last_realtime = realtime_now;
 138         sendMotorUpdate();
 139         return;
 140     }
 141
 142     const double deltaSim = pkt->timestamp - last_timestamp;  // in seconds
 143     if (deltaSim < 0) { // don't use old packet
 144         return;
 145     }
 146
 147     int16_t x,y,z;
 148     x = constrain(-pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE, -32767, 32767);
 149     y = constrain(-pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE, -32767, 32767);
 150     z = constrain(-pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE, -32767, 32767);
 151     virtualAccSet(virtualAccDev, x, y, z);
 152 //    printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
 153
 154     x = constrain(pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG, -32767, 32767);
 155     y = constrain(-pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG, -32767, 32767);
 156     z = constrain(-pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG, -32767, 32767);
 157     virtualGyroSet(virtualGyroDev, x, y, z);
 158 //    printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
 159
 160     // temperature in 0.01 C = 25 deg
 161     virtualBaroSet(pkt->pressure, 2500);
 162 #if !defined(USE_IMU_CALC)
 163 #if defined(SET_IMU_FROM_EULER)
 164     // set from Euler
 165     double qw = pkt->imu_orientation_quat[0];
 166     double qx = pkt->imu_orientation_quat[1];
 167     double qy = pkt->imu_orientation_quat[2];
 168     double qz = pkt->imu_orientation_quat[3];
 169     double ysqr = qy * qy;
 170     double xf, yf, zf;
 171
 172     // roll (x-axis rotation)
 173     double t0 = +2.0 * (qw * qx + qy * qz);
 174     double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
 175     xf = atan2(t0, t1) * RAD2DEG;
 176
 177     // pitch (y-axis rotation)
 178     double t2 = +2.0 * (qw * qy - qz * qx);
 179     t2 = t2 > 1.0 ? 1.0 : t2;
 180     t2 = t2 < -1.0 ? -1.0 : t2;
 181     yf = asin(t2) * RAD2DEG; // from wiki
 182
 183     // yaw (z-axis rotation)
 184     double t3 = +2.0 * (qw * qz + qx * qy);
 185     double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
 186     zf = atan2(t3, t4) * RAD2DEG;
 187     imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
 188 #else
 189     imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
 190 #endif
 191 #endif
 192
 193 #if defined(USE_VIRTUAL_GPS)
 194     const double longitude = pkt->position_xyz[0];
 195     const double latitude = pkt->position_xyz[1];
 196     const double altitude = pkt->position_xyz[2];
 197     const double speed = sqrt(sq(pkt->velocity_xyz[0]) + sq(pkt->velocity_xyz[1]));
 198     const double speed3D = sqrt(sq(pkt->velocity_xyz[0]) + sq(pkt->velocity_xyz[1]) + sq(pkt->velocity_xyz[2]));
 199     double course = atan2(pkt->velocity_xyz[0], pkt->velocity_xyz[1]) * RAD2DEG;
 200     if (course < 0.0) {
 201         course += 360.0;
 202     }
 203     setVirtualGPS(latitude, longitude, altitude, speed, speed3D, course);
 204 #endif
 205
 206 #if defined(SIMULATOR_IMU_SYNC)
 207     imuSetHasNewData(deltaSim*1e6);
 208     imuUpdateAttitude(micros());
 209 #endif
 210
 211     if (deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
 212 //        simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
 213         struct timespec out_ts;
 214         timeval_sub(&out_ts, &now_ts, &last_ts);
 215         simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
 216     }
 217 //    printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
 218
 219     last_timestamp = pkt->timestamp;
 220     last_realtime = micros64_real();
 221
 222     last_ts.tv_sec = now_ts.tv_sec;
 223     last_ts.tv_nsec = now_ts.tv_nsec;
 224
 225     pthread_mutex_unlock(&updateLock); // can send PWM output now
 226
 227 #if defined(SIMULATOR_GYROPID_SYNC)
 228     pthread_mutex_unlock(&mainLoopLock); // can run main loop
 229 #endif
 230 }
 231
 232 static void* udpThread(void* data)
 233 {
 234     UNUSED(data);
 235     int n = 0;
 236
 237     while (workerRunning) {
 238         n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
 239         if (n == sizeof(fdm_packet)) {
 240             if (!fdm_received) {
 241                 printf("[SITL] new fdm %d t:%f from %s:%d\n", n, fdmPkt.timestamp, inet_ntoa(stateLink.recv.sin_addr), stateLink.recv.sin_port);
 242                 fdm_received = true;
 243             }
 244             updateState(&fdmPkt);
 245         }
 246     }
 247
 248     printf("udpThread end!!\n");
 249     return NULL;
 250 }
 251
 252 static float readRCSITL(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
 253 {
 254     UNUSED(rxRuntimeState);
 255     return rcPkt.channels[channel];
 256 }
 257
 258 static uint8_t rxRCFrameStatus(rxRuntimeState_t *rxRuntimeState)
 259 {
 260     UNUSED(rxRuntimeState);
 261     return RX_FRAME_COMPLETE;
 262 }
 263
 264 static void *udpRCThread(void *data)
 265 {
 266     UNUSED(data);
 267     int n = 0;
 268
 269     while (workerRunning) {
 270         n = udpRecv(&rcLink, &rcPkt, sizeof(rc_packet), 100);
 271         if (n == sizeof(rc_packet)) {
 272             if (!rc_received) {
 273                 printf("[SITL] new rc %d: t:%f AETR: %d %d %d %d AUX1-4: %d %d %d %d\n", n, rcPkt.timestamp,
 274                     rcPkt.channels[0], rcPkt.channels[1],rcPkt.channels[2],rcPkt.channels[3],
 275                     rcPkt.channels[4], rcPkt.channels[5],rcPkt.channels[6],rcPkt.channels[7]);
 276
 277                 rxRuntimeState.channelCount = SIMULATOR_MAX_RC_CHANNELS;
 278                 rxRuntimeState.rcReadRawFn = readRCSITL;
 279                 rxRuntimeState.rcFrameStatusFn = rxRCFrameStatus;
 280
 281                 rxRuntimeState.rxProvider = RX_PROVIDER_UDP;
 282                 rc_received = true;
 283             }
 284         }
 285     }
 286
 287     printf("udpRCThread end!!\n");
 288     return NULL;
 289 }
 290
 291 static void* tcpThread(void* data)
 292 {
 293     UNUSED(data);
 294
 295     dyad_init();
 296     dyad_setTickInterval(0.2f);
 297     dyad_setUpdateTimeout(0.01f);
 298
 299     while (workerRunning) {
 300         dyad_update();
 301     }
 302
 303     dyad_shutdown();
 304     printf("tcpThread end!!\n");
 305     return NULL;
 306 }
 307
 308 // system
 309 void systemInit(void)
 310 {
 311     int ret;
 312
 313     clock_gettime(CLOCK_MONOTONIC, &start_time);
 314     printf("[system]Init...\n");
 315
 316     SystemCoreClock = 500 * 1e6; // virtual 500MHz
 317
 318     if (pthread_mutex_init(&updateLock, NULL) != 0) {
 319         printf("Create updateLock error!\n");
 320         exit(1);
 321     }
 322
 323     if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
 324         printf("Create mainLoopLock error!\n");
 325         exit(1);
 326     }
 327
 328     ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
 329     if (ret != 0) {
 330         printf("Create tcpWorker error!\n");
 331         exit(1);
 332     }
 333
 334     ret = udpInit(&pwmLink, simulator_ip, PORT_PWM, false);
 335     printf("[SITL] init PwmOut UDP link to gazebo %s:%d...%d\n", simulator_ip, PORT_PWM, ret);
 336
 337     ret = udpInit(&pwmRawLink, simulator_ip, PORT_PWM_RAW, false);
 338     printf("[SITL] init PwmOut UDP link to RF9 %s:%d...%d\n", simulator_ip, PORT_PWM_RAW, ret);
 339
 340     ret = udpInit(&stateLink, NULL, PORT_STATE, true);
 341     printf("[SITL] start UDP server @%d...%d\n", PORT_STATE, ret);
 342
 343     ret = udpInit(&rcLink, NULL, PORT_RC, true);
 344     printf("[SITL] start UDP server for RC input @%d...%d\n", PORT_RC, ret);
 345
 346     ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
 347     if (ret != 0) {
 348         printf("Create udpWorker error!\n");
 349         exit(1);
 350     }
 351
 352     ret = pthread_create(&udpWorkerRC, NULL, udpRCThread, NULL);
 353     if (ret != 0) {
 354         printf("Create udpRCThread error!\n");
 355         exit(1);
 356     }
 357 }
 358
 359 void systemReset(void)
 360 {
 361     printf("[system]Reset!\n");
 362     workerRunning = false;
 363     pthread_join(tcpWorker, NULL);
 364     pthread_join(udpWorker, NULL);
 365     exit(0);
 366 }
 367 void systemResetToBootloader(bootloaderRequestType_e requestType)
 368 {
 369     UNUSED(requestType);
 370
 371     printf("[system]ResetToBootloader!\n");
 372     workerRunning = false;
 373     pthread_join(tcpWorker, NULL);
 374     pthread_join(udpWorker, NULL);
 375     exit(0);
 376 }
 377
 378 void timerInit(void)
 379 {
 380     printf("[timer]Init...\n");
 381 }
 382
 383 void failureMode(failureMode_e mode)
 384 {
 385     printf("[failureMode]!!! %d\n", mode);
 386     while (1);
 387 }
 388
 389 void indicateFailure(failureMode_e mode, int repeatCount)
 390 {
 391     UNUSED(repeatCount);
 392     printf("Failure LED flash for: [failureMode]!!! %d\n", mode);
 393 }
 394
 395 // Time part
 396 // Thanks ArduPilot
 397 uint64_t nanos64_real(void)
 398 {
 399     struct timespec ts;
 400     clock_gettime(CLOCK_MONOTONIC, &ts);
 401     return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec);
 402 }
 403
 404 uint64_t micros64_real(void)
 405 {
 406     struct timespec ts;
 407     clock_gettime(CLOCK_MONOTONIC, &ts);
 408     return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
 409 }
 410
 411 uint64_t millis64_real(void)
 412 {
 413     struct timespec ts;
 414     clock_gettime(CLOCK_MONOTONIC, &ts);
 415     return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
 416 }
 417
 418 uint64_t micros64(void)
 419 {
 420     static uint64_t last = 0;
 421     static uint64_t out = 0;
 422     uint64_t now = nanos64_real();
 423
 424     out += (now - last) * simRate;
 425     last = now;
 426
 427     return out*1e-3;
 428 //    return micros64_real();
 429 }
 430
 431 uint64_t millis64(void)
 432 {
 433     static uint64_t last = 0;
 434     static uint64_t out = 0;
 435     uint64_t now = nanos64_real();
 436
 437     out += (now - last) * simRate;
 438     last = now;
 439
 440     return out*1e-6;
 441 //    return millis64_real();
 442 }
 443
 444 uint32_t micros(void)
 445 {
 446     return micros64() & 0xFFFFFFFF;
 447 }
 448
 449 uint32_t millis(void)
 450 {
 451     return millis64() & 0xFFFFFFFF;
 452 }
 453
 454 int32_t clockCyclesToMicros(int32_t clockCycles)
 455 {
 456     return clockCycles;
 457 }
 458
 459 int32_t clockCyclesTo10thMicros(int32_t clockCycles)
 460 {
 461     return clockCycles;
 462 }
 463
 464 int32_t clockCyclesTo100thMicros(int32_t clockCycles)
 465 {
 466     return clockCycles;
 467 }
 468
 469 uint32_t clockMicrosToCycles(uint32_t micros)
 470 {
 471     return micros;
 472 }
 473
 474 uint32_t getCycleCounter(void)
 475 {
 476     return (uint32_t) (micros64() & 0xFFFFFFFF);
 477 }
 478
 479 static void microsleep(uint32_t usec)
 480 {
 481     struct timespec ts;
 482     ts.tv_sec = 0;
 483     ts.tv_nsec = usec*1000UL;
 484     while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
 485 }
 486
 487 void delayMicroseconds(uint32_t us)
 488 {
 489     microsleep(us / simRate);
 490 }
 491
 492 void delayMicroseconds_real(uint32_t us)
 493 {
 494     microsleep(us);
 495 }
 496
 497 void delay(uint32_t ms)
 498 {
 499     uint64_t start = millis64();
 500
 501     while ((millis64() - start) < ms) {
 502         microsleep(1000);
 503     }
 504 }
 505
 506 // Subtract the ‘struct timespec’ values X and Y,  storing the result in RESULT.
 507 // Return 1 if the difference is negative, otherwise 0.
 508 // result = x - y
 509 // from: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
 510 int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y)
 511 {
 512     unsigned int s_carry = 0;
 513     unsigned int ns_carry = 0;
 514     // Perform the carry for the later subtraction by updating y.
 515     if (x->tv_nsec < y->tv_nsec) {
 516         int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
 517         ns_carry += 1000000000 * nsec;
 518         s_carry += nsec;
 519     }
 520
 521     // Compute the time remaining to wait. tv_usec is certainly positive.
 522     result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
 523     result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
 524
 525     // Return 1 if result is negative.
 526     return x->tv_sec < y->tv_sec;
 527 }
 528
 529 // PWM part
 530 static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
 531
 532 // real value to send
 533 static int16_t motorsPwm[MAX_SUPPORTED_MOTORS];
 534 static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
 535 static int16_t idlePulse;
 536
 537 void servoDevInit(const servoDevConfig_t *servoConfig)
 538 {
 539     printf("[SITL] Init servos num %d rate %d center %d\n", MAX_SUPPORTED_SERVOS,
 540            servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
 541     for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
 542         servos[servoIndex].enabled = true;
 543     }
 544 }
 545
 546 static motorDevice_t pwmMotorDevice; // Forward
 547
 548 pwmOutputPort_t *pwmGetMotors(void)
 549 {
 550     return motors;
 551 }
 552
 553 static float pwmConvertFromExternal(uint16_t externalValue)
 554 {
 555     return (float)externalValue;
 556 }
 557
 558 static uint16_t pwmConvertToExternal(float motorValue)
 559 {
 560     return (uint16_t)motorValue;
 561 }
 562
 563 static void pwmDisableMotors(void)
 564 {
 565     pwmMotorDevice.enabled = false;
 566 }
 567
 568 static bool pwmEnableMotors(void)
 569 {
 570     pwmMotorDevice.enabled = true;
 571
 572     return true;
 573 }
 574
 575 static void pwmWriteMotor(uint8_t index, float value)
 576 {
 577     if (pthread_mutex_trylock(&updateLock) != 0) return;
 578
 579     if (index < MAX_SUPPORTED_MOTORS) {
 580         motorsPwm[index] = value - idlePulse;
 581     }
 582
 583     if (index < pwmRawPkt.motorCount) {
 584         pwmRawPkt.pwm_output_raw[index] = value;
 585     }
 586
 587     pthread_mutex_unlock(&updateLock); // can send PWM output now
 588 }
 589
 590 static void pwmWriteMotorInt(uint8_t index, uint16_t value)
 591 {
 592     pwmWriteMotor(index, (float)value);
 593 }
 594
 595 static void pwmShutdownPulsesForAllMotors(void)
 596 {
 597     pwmMotorDevice.enabled = false;
 598 }
 599
 600 static bool pwmIsMotorEnabled(unsigned index)
 601 {
 602     return motors[index].enabled;
 603 }
 604
 605 static void pwmCompleteMotorUpdate(void)
 606 {
 607     // send to simulator
 608     // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
 609
 610     double outScale = 1000.0;
 611     if (featureIsEnabled(FEATURE_3D)) {
 612         outScale = 500.0;
 613     }
 614
 615     pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
 616     pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
 617     pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
 618     pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
 619
 620     // get one "fdm_packet" can only send one "servo_packet"!!
 621     if (pthread_mutex_trylock(&updateLock) != 0) return;
 622     udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
 623 //    printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
 624     udpSend(&pwmRawLink, &pwmRawPkt, sizeof(servo_packet_raw));
 625 }
 626
 627 void pwmWriteServo(uint8_t index, float value)
 628 {
 629     servosPwm[index] = value;
 630     if (index + pwmRawPkt.motorCount < SIMULATOR_MAX_PWM_CHANNELS) {
 631         // In pwmRawPkt, we put servo right after the motors.
 632         pwmRawPkt.pwm_output_raw[index + pwmRawPkt.motorCount] = value;
 633     }
 634 }
 635
 636 static const motorVTable_t vTable = {
 637     .postInit = motorPostInitNull,
 638     .convertExternalToMotor = pwmConvertFromExternal,
 639     .convertMotorToExternal = pwmConvertToExternal,
 640     .enable = pwmEnableMotors,
 641     .disable = pwmDisableMotors,
 642     .isMotorEnabled = pwmIsMotorEnabled,
 643     .decodeTelemetry = motorDecodeTelemetryNull,
 644     .write = pwmWriteMotor,
 645     .writeInt = pwmWriteMotorInt,
 646     .updateComplete = pwmCompleteMotorUpdate,
 647     .shutdown = pwmShutdownPulsesForAllMotors,
 648     .requestTelemetry = NULL,
 649     .isMotorIdle = NULL,
 650
 651 };
 652
 653 bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse)
 654 {
 655     UNUSED(motorConfig);
 656
 657     if (!device) {
 658         return false;
 659     }
 660     device->vTable = &vTable;
 661     const uint8_t motorCount = device->count;
 662     printf("Initialized motor count %d\n", motorCount);
 663     pwmRawPkt.motorCount = motorCount;
 664
 665     idlePulse = _idlePulse;
 666
 667     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 668         motors[motorIndex].enabled = true;
 669     }
 670
 671     return true;
 672 }
 673
 674 // ADC part
 675 uint16_t adcGetChannel(uint8_t channel)
 676 {
 677     UNUSED(channel);
 678     return 0;
 679 }
 680
 681 // stack part
 682 char _estack;
 683 char _Min_Stack_Size;
 684
 685 // virtual EEPROM
 686 static FILE *eepromFd = NULL;
 687
 688 bool loadEEPROMFromFile(void)
 689 {
 690     if (eepromFd != NULL) {
 691         fprintf(stderr, "[FLASH_Unlock] eepromFd != NULL\n");
 692         return false;
 693     }
 694
 695     // open or create
 696     eepromFd = fopen(EEPROM_FILENAME, "r+");
 697     if (eepromFd != NULL) {
 698         // obtain file size:
 699         fseek(eepromFd, 0, SEEK_END);
 700         size_t lSize = ftell(eepromFd);
 701         rewind(eepromFd);
 702
 703         size_t n = fread(eepromData, 1, sizeof(eepromData), eepromFd);
 704         if (n == lSize) {
 705             printf("[FLASH_Unlock] loaded '%s', size = %ld / %ld\n", EEPROM_FILENAME, lSize, sizeof(eepromData));
 706         } else {
 707             fprintf(stderr, "[FLASH_Unlock] failed to load '%s'\n", EEPROM_FILENAME);
 708             return false;
 709         }
 710     } else {
 711         printf("[FLASH_Unlock] created '%s', size = %ld\n", EEPROM_FILENAME, sizeof(eepromData));
 712         if ((eepromFd = fopen(EEPROM_FILENAME, "w+")) == NULL) {
 713             fprintf(stderr, "[FLASH_Unlock] failed to create '%s'\n", EEPROM_FILENAME);
 714             return false;
 715         }
 716
 717         if (fwrite(eepromData, sizeof(eepromData), 1, eepromFd) != 1) {
 718             fprintf(stderr, "[FLASH_Unlock] write failed: %s\n", strerror(errno));
 719             return false;
 720         }
 721     }
 722     return true;
 723 }
 724
 725 void configUnlock(void)
 726 {
 727     loadEEPROMFromFile();
 728 }
 729
 730 void configLock(void)
 731 {
 732     // flush & close
 733     if (eepromFd != NULL) {
 734         fseek(eepromFd, 0, SEEK_SET);
 735         fwrite(eepromData, 1, sizeof(eepromData), eepromFd);
 736         fclose(eepromFd);
 737         eepromFd = NULL;
 738         printf("[FLASH_Lock] saved '%s'\n", EEPROM_FILENAME);
 739     } else {
 740         fprintf(stderr, "[FLASH_Lock] eeprom is not unlocked\n");
 741     }
 742 }
 743
 744 configStreamerResult_e configWriteWord(uintptr_t address, config_streamer_buffer_type_t *buffer)
 745 {
 746     STATIC_ASSERT(CONFIG_STREAMER_BUFFER_SIZE == sizeof(uint32_t), "CONFIG_STREAMER_BUFFER_SIZE does not match written size");
 747
 748     if ((address >= (uintptr_t)eepromData) && (address + sizeof(uint32_t) <= (uintptr_t)ARRAYEND(eepromData))) {
 749         memcpy((void*)address, buffer, sizeof(config_streamer_buffer_type_t));
 750         printf("[FLASH_ProgramWord]%p = %08x\n", (void*)address, *((uint32_t*)address));
 751     } else {
 752         printf("[FLASH_ProgramWord]%p out of range!\n", (void*)address);
 753     }
 754     return CONFIG_RESULT_SUCCESS;
 755 }
 756
 757 void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 758 {
 759     UNUSED(io);
 760     UNUSED(cfg);
 761     printf("IOConfigGPIO\n");
 762 }
 763
 764 void spektrumBind(rxConfig_t *rxConfig)
 765 {
 766     UNUSED(rxConfig);
 767     printf("spektrumBind\n");
 768 }
 769
 770 void debugInit(void)
 771 {
 772     printf("debugInit\n");
 773 }
 774
 775 void unusedPinsInit(void)
 776 {
 777     printf("unusedPinsInit\n");
 778 }
 779
 780 void IOHi(IO_t io)
 781 {
 782     UNUSED(io);
 783 }
 784
 785 void IOLo(IO_t io)
 786 {
 787     UNUSED(io);
 788 }
 789
 790 void IOInitGlobal(void)
 791 {
 792     // NOOP
 793 }
 794
 795 IO_t IOGetByTag(ioTag_t tag)
 796 {
 797     UNUSED(tag);
 798     return NULL;
 799 }
 800
 801 const mcuTypeInfo_t *getMcuTypeInfo(void)
 802 {
 803     static const mcuTypeInfo_t info = { .id = MCU_TYPE_SIMULATOR, .name = "SIMULATOR" };
 804     return &info;
 805 }