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 Hirth 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_Hirth.h"
      24             : 
      25             : using namespace SITL;
      26             : 
      27             : // assume SERVO3 is throttle
      28             : #define HIRTH_RPM_INDEX 2
      29             : 
      30       30210 : void EFI_Hirth::update_receive()
      31             : {
      32       30210 :     const ssize_t num_bytes_read = read_from_autopilot((char*)&receive_buf[receive_buf_ofs], ARRAY_SIZE(receive_buf) - receive_buf_ofs);
      33       30210 :     if (num_bytes_read < 0) {
      34           0 :         return;
      35             :     }
      36       30210 :     receive_buf_ofs += num_bytes_read;
      37             : 
      38       30210 :     if (receive_buf_ofs < 1) {
      39       29601 :         return;
      40             :     }
      41             : 
      42         609 :     const uint8_t expected_bytes_in_message = receive_buf[0];
      43             : 
      44         609 :     if (expected_bytes_in_message == 0) {
      45           0 :         AP_HAL::panic("zero bytes expected is unexpected");
      46             :     }
      47             : 
      48         609 :     if (expected_bytes_in_message > ARRAY_SIZE(receive_buf)) {
      49           0 :         AP_HAL::panic("Unexpectedly large byte count");
      50             :     }
      51             : 
      52         609 :     if (receive_buf_ofs < expected_bytes_in_message) {
      53           0 :         return;
      54             :     }
      55             : 
      56             :     // checksum is sum of all bytes except the received checksum:
      57         609 :     const uint8_t expected_checksum = 256U - crc_sum_of_bytes(receive_buf, expected_bytes_in_message-1);
      58         609 :     const uint8_t received_checksum = receive_buf[expected_bytes_in_message-1];
      59         609 :     if (expected_checksum == received_checksum) {
      60         609 :         PacketCode received_packet_code = PacketCode(receive_buf[1]);
      61         609 :         if (received_packet_code == PacketCode::SetValues) {
      62             :             // do this synchronously for now
      63          46 :             handle_set_values();
      64         563 :         } else if (uint8_t(received_packet_code) == 0x04 ||
      65         176 :                    uint8_t(received_packet_code) == 0x0B ||
      66             :                    uint8_t(received_packet_code) == 0x0D) {
      67         563 :             assert_receive_size(3);
      68         563 :             if (requested_data_record.time_ms != 0) {
      69           0 :                 AP_HAL::panic("Requesting too fast?");
      70             :             }
      71         563 :             requested_data_record.code = received_packet_code;
      72         563 :             requested_data_record.time_ms = AP_HAL::millis();
      73             :         } else {
      74           0 :             AP_HAL::panic("Invalid packet code");
      75             :         }
      76             :     } else {
      77           0 :         AP_HAL::panic("checksum failed");
      78             :         // simply throw these bytes away.  What the actual device does in the
      79             :         // face of weird data is unknown.
      80             :     }
      81         609 :     memmove(&receive_buf[0], &receive_buf[expected_bytes_in_message], receive_buf_ofs - expected_bytes_in_message);
      82         609 :     receive_buf_ofs -= expected_bytes_in_message;
      83             : }
      84             : 
      85         609 : void EFI_Hirth::assert_receive_size(uint8_t receive_size)
      86             : {
      87         609 :     if (receive_buf[0] != receive_size) {
      88           0 :         AP_HAL::panic("Expected %u message size, got %u message size", receive_size, receive_buf[0]);
      89             :     }
      90         609 : }
      91             : 
      92          46 : void EFI_Hirth::handle_set_values()
      93             : {
      94          46 :     assert_receive_size(23);
      95             :     static_assert(sizeof(settings) == 20, "correct number of bytes in settings");
      96          46 :     memcpy((void*)&settings, &receive_buf[2], sizeof(settings));
      97             : 
      98             :     // send ACK for set-values
      99          46 :     constexpr uint8_t set_values_ack[] {
     100             :         3,  // length
     101             :         uint8_t(PacketCode::SetValues),  // code
     102             :         3 + uint8_t(PacketCode::SetValues)
     103             :     };
     104          46 :     write_to_autopilot((const char*)set_values_ack, sizeof(set_values_ack));
     105          46 : }
     106             : 
     107       30210 : void EFI_Hirth::update_send(const class Aircraft &aircraft)
     108             : {
     109       30210 :     if (requested_data_record.time_ms == 0) {
     110             :         // no outstanding request
     111       16231 :         return;
     112             :     }
     113       13979 :     if (AP_HAL::millis() - requested_data_record.time_ms < 20) {
     114             :         // 20ms to service a request
     115       13416 :         return;
     116             :     }
     117         563 :     requested_data_record.time_ms = 0;
     118             : 
     119         563 :     switch (requested_data_record.code) {
     120         210 :     case PacketCode::DataRecord1:
     121         210 :         send_record1(aircraft);
     122         210 :         break;
     123         177 :     case PacketCode::DataRecord2:
     124         177 :         send_record2();
     125         177 :         break;
     126         176 :     case PacketCode::DataRecord3:
     127         176 :         send_record3();
     128         176 :         break;
     129           0 :     default:
     130           0 :         AP_HAL::panic("Unknown data record (%u) requested", (unsigned)requested_data_record.code);
     131             :     }
     132             : }
     133             : 
     134       30210 : void EFI_Hirth::update_engine_model(const class Aircraft &aircraft)
     135             : {
     136       30210 :     auto sitl = AP::sitl();
     137             : 
     138       30210 :     const float ambient = aircraft.ambient_outside_temperature_degC();
     139             : 
     140       30210 :     const uint32_t now_ms = AP_HAL::millis();
     141             : 
     142       30210 :     const float delta_t = (now_ms - engine.last_update_ms) * 1e-6;
     143       30210 :     engine.last_update_ms = now_ms;
     144             : 
     145             :     // lose heat to environment (air-cooling due to airspeed and prop
     146             :     // airflow could be taken into account here)
     147       30210 :     const float ENV_LOSS_FACTOR = 25;
     148       30210 :     engine.cht1_temperature -= (engine.cht1_temperature - ambient) * delta_t * ENV_LOSS_FACTOR;
     149       30210 :     engine.cht2_temperature -= (engine.cht2_temperature - ambient) * delta_t * ENV_LOSS_FACTOR;
     150             : 
     151       30210 :     const float rpm = sitl->state.rpm[HIRTH_RPM_INDEX];
     152       30210 :     const float RPM_GAIN_FACTOR_CHT1 = 10;
     153       30210 :     const float RPM_GAIN_FACTOR_CHT2 = 8;
     154       30210 :     engine.cht1_temperature += rpm * delta_t * RPM_GAIN_FACTOR_CHT1;
     155       30210 :     engine.cht2_temperature += rpm * delta_t * RPM_GAIN_FACTOR_CHT2;
     156       30210 : }
     157             : 
     158         149 : void EFI_Hirth::init(const class Aircraft &aircraft)
     159             : {
     160             :     // auto sitl = AP::sitl();
     161             : 
     162         149 :     const float temperature = aircraft.ambient_outside_temperature_degC();
     163         149 :     if (is_zero(temperature)) {
     164             :         // defer until the baro has had a chance to update....
     165         148 :         return;
     166             :     }
     167             : 
     168           1 :     engine.cht1_temperature = temperature;
     169           1 :     engine.cht2_temperature = temperature;
     170             : 
     171           1 :     init_done = true;
     172             : }
     173             : 
     174       31243 : void EFI_Hirth::update(const class Aircraft &aircraft)
     175             : {
     176       31243 :     const auto *sitl = AP::sitl();
     177       31243 :     if (!sitl || sitl->efi_type != SIM::EFI_TYPE_HIRTH) {
     178        1033 :         return;
     179             :     }
     180             : 
     181       30210 :     if (!init_done) {
     182         149 :         init(aircraft);
     183             :     }
     184             : 
     185             :     // update throttle; interim thing to make life a little more interesting
     186       30210 :     throttle = 0.9 * throttle + 0.1 * settings.throttle/10;
     187             : 
     188       30210 :     update_engine_model(aircraft);
     189             : 
     190       30210 :     update_receive();
     191       30210 :     update_send(aircraft);
     192             : }
     193             : 
     194         210 : uint16_t EFI_Hirth::engine_status_field_value() const
     195             : {
     196             :     return (
     197             :         0U << 0 |  // engine temperature sensor
     198             :         1U << 1 |  // air temperature sensor
     199             :         1U << 2 |  // air pressure sensor
     200             :         1U << 3    // throttle sensor OK
     201         210 :         );
     202             : }
     203             : 
     204         210 : void SITL::EFI_Hirth::send_record1(const class Aircraft &aircraft)
     205             : {
     206         210 :     const auto *sitl = AP::sitl();
     207             : 
     208             :     // notionally the field updates should happen in the update()
     209             :     // method, but here to save CPU for now:
     210         210 :     auto &r = packed_record1.record;
     211         210 :     r.engine_status = engine_status_field_value();
     212         210 :     r.rpm = sitl->state.rpm[HIRTH_RPM_INDEX];
     213         210 :     r.air_temperature = aircraft.ambient_outside_temperature_degC();
     214         210 :     r.throttle = settings.throttle / 10;  // just echo this back
     215             : 
     216         210 :     packed_record1.update_checksum();
     217             : 
     218         210 :     write_to_autopilot((char*)&packed_record1, sizeof(packed_record1));
     219             : 
     220             :     ASSERT_STORAGE_SIZE(Record1, 84);
     221         210 : }
     222             : 
     223         177 : void SITL::EFI_Hirth::send_record2()
     224             : {
     225         177 :     const auto *sitl = AP::sitl();
     226             : 
     227             :     // notionally the field updates should happen in the update()
     228             :     // method, but here to save CPU for now:
     229         177 :     auto &r = packed_record2.record;
     230         177 :     r.throttle_percent_times_10 = throttle * 10.0;
     231         177 :     r.fuel_consumption = ((MAX(sitl->state.rpm[HIRTH_RPM_INDEX] - 1500.0, 0)) /2200.0) * 10;  // from log, very rough
     232             : 
     233         177 :     packed_record2.update_checksum();
     234             : 
     235         177 :     write_to_autopilot((char*)&packed_record2, sizeof(packed_record2));
     236             : 
     237             :     ASSERT_STORAGE_SIZE(Record2, 98);
     238         177 : }
     239             : 
     240             : 
     241         176 : void SITL::EFI_Hirth::send_record3()
     242             : {
     243             :     // notionally the field updates should happen in the update()
     244             :     // method, but here to save CPU for now:
     245         176 :     auto &r = packed_record3.record;
     246         176 :     r.excess_temperature_1 = engine.cht1_temperature;  // cht1
     247         176 :     r.excess_temperature_2 = engine.cht2_temperature;  // cht2
     248         176 :     r.excess_temperature_3 = 39;  // egt1
     249         176 :     r.excess_temperature_4 = 41;  // egt2
     250             : 
     251         176 :     packed_record3.update_checksum();
     252             : 
     253         176 :     write_to_autopilot((char*)&packed_record3, sizeof(packed_record3));
     254             : 
     255             :     ASSERT_STORAGE_SIZE(Record3, 100);
     256         176 : }