Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   simulate MegaSquirt EFI system
      17             : */
      18             : 
      19             : #include "SIM_Aircraft.h"
      20             : #include <SITL/SITL.h>
      21             : #include <AP_HAL/utility/sparse-endian.h>
      22             : #include <stdio.h>
      23             : #include "SIM_EFI_MegaSquirt.h"
      24             : 
      25             : using namespace SITL;
      26             : 
      27         470 : static uint32_t CRC32_MS(const uint8_t *buf, uint32_t len)
      28             : {
      29         470 :     uint32_t crc = 0;
      30       31960 :     while (len--) {
      31       31490 :         crc ^= ~0U;
      32       31490 :         crc = crc_crc32(crc, buf++, 1);
      33       31490 :         crc ^= ~0U;
      34             :     }
      35         470 :     return crc;
      36             : }
      37             : 
      38       31243 : void EFI_MegaSquirt::update(const class Aircraft &aircraft)
      39             : {
      40       31243 :     auto sitl = AP::sitl();
      41       31243 :     if (!sitl || sitl->efi_type != SIM::EFI_TYPE_MS) {
      42        1009 :         return;
      43             :     }
      44       30234 :     const float rpm = sitl->state.rpm[2];
      45             : 
      46       30234 :     tps = 0.9 * tps + 0.1 * (rpm / 7000) * 100;
      47             : 
      48       30234 :     table7.rpm = rpm;
      49       30234 :     table7.fuelload = 20;
      50       30234 :     table7.dwell = 2.0;
      51       30234 :     table7.baro_hPa = 1000;
      52       30234 :     table7.map_hPa = 895;
      53       30234 :     table7.mat_cF = C_TO_F(aircraft.ambient_outside_temperature_degC()) * 10;
      54       30234 :     table7.fuelPressure = 6280;
      55       30234 :     table7.throttle_pos = tps * 10;
      56       30234 :     table7.ct_cF = 3940;
      57       30234 :     table7.afr_target1 = 148;
      58             : 
      59             :     // receive command
      60       33289 :     while (ofs < sizeof(r_command)) {
      61       33054 :         if (read_from_autopilot((char*)&buf[ofs], 1) != 1) {
      62       29999 :             break;
      63             :         }
      64        3055 :         switch (ofs) {
      65         235 :         case 0:
      66         235 :             if (buf[ofs] == 0) {
      67         235 :                 ofs++;
      68             :             }
      69         235 :             break;
      70         235 :         case 1:
      71         235 :             if (buf[ofs] != 7) {
      72           0 :                 ofs = 0;
      73             :             } else {
      74         235 :                 ofs++;
      75             :             }
      76         235 :             break;
      77         235 :         case 2:
      78         235 :             if (buf[ofs] != 0x72) {
      79           0 :                 ofs = 0;
      80             :             } else {
      81         235 :                 ofs++;
      82             :             }
      83         235 :             break;
      84         235 :         case 3:
      85         235 :             if (buf[ofs] != 0x00) {
      86           0 :                 ofs = 0;
      87             :             } else {
      88         235 :                 ofs++;
      89             :             }
      90         235 :             break;
      91         235 :         case 4:
      92         235 :             if (buf[ofs] != 0x07) {
      93           0 :                 ofs = 0;
      94             :             } else {
      95         235 :                 ofs++;
      96             :             }
      97         235 :             break;
      98        1880 :         case 5 ... 12:
      99        1880 :             ofs++;
     100        1880 :             break;
     101             :         }
     102             :     }
     103       30234 :     if (ofs >= sizeof(r_command)) {
     104             :         // check CRC
     105         235 :         uint32_t crc = CRC32_MS(&buf[2], sizeof(r_command)-6);
     106         235 :         uint32_t crc2 = be32toh(r_command.crc);
     107         235 :         if (crc == crc2) {
     108         235 :             send_table();
     109             :         } else {
     110           0 :             printf("BAD EFI CRC: 0x%08x 0x%08x\n", (unsigned int)crc, (unsigned int)crc2);
     111             :         }
     112         235 :         ofs = 0;
     113             :     }
     114             : }
     115             : 
     116             : /*
     117             :   send table response
     118             :  */
     119         235 : void EFI_MegaSquirt::send_table(void)
     120             : {
     121         235 :     uint16_t table_offset = be16toh(r_command.table_offset);
     122         235 :     uint16_t table_size = be16toh(r_command.table_size);
     123             : 
     124         235 :     if (table_offset >= sizeof(table7)) {
     125           0 :         printf("EFI_MS: bad table_offset %u\n", table_offset);
     126           0 :         return;
     127             :     }
     128         235 :     if (table_size+table_offset > sizeof(table7)) {
     129         235 :         table_size = sizeof(table7) - table_offset;
     130             :     }
     131             : 
     132         235 :     uint16_t len = htobe16(table_size+1);
     133         235 :     uint8_t outbuf[1+table_size];
     134         235 :     outbuf[0] = 0;
     135         235 :     swab(table_offset+(const uint8_t *)&table7, &outbuf[1], table_size);
     136             : 
     137         235 :     write_to_autopilot((const char*)&len, sizeof(len));
     138         235 :     write_to_autopilot((const char*)outbuf, sizeof(outbuf));
     139         235 :     uint32_t crc = htobe32(CRC32_MS(outbuf, sizeof(outbuf)));
     140         235 :     write_to_autopilot((const char *)&crc, sizeof(crc));
     141         235 : }