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