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