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