SITL: add RichenPower generator simulator

libraries/SITL/SIM_Aircraft.cpp

@@ -811,6 +811,11 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
     if (precland && precland->is_enabled()) {
         precland->update(get_location(), get_position());
     }
+
+    // update RichenPower generator
+    if (richenpower) {
+        richenpower->update(input);
+    }
 }
 
 void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)

libraries/SITL/SIM_Aircraft.h

@@ -28,6 +28,7 @@
 #include "SIM_Gripper_EPM.h"
 #include "SIM_Parachute.h"
 #include "SIM_Precland.h"
+#include "SIM_RichenPower.h"
 #include "SIM_Buzzer.h"
 #include <Filter/Filter.h>
 
@@ -137,6 +138,7 @@ public:
     void set_buzzer(Buzzer *_buzzer) { buzzer = _buzzer; }
     void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
     void set_parachute(Parachute *_parachute) { parachute = _parachute; }
+    void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
     void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
     void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; }
     void set_precland(SIM_Precland *_precland);
@@ -296,6 +298,7 @@ private:
     Gripper_Servo *gripper;
     Gripper_EPM *gripper_epm;
     Parachute *parachute;
+    RichenPower *richenpower;
     SIM_Precland *precland;
 };
 

libraries/SITL/SIM_RichenPower.cpp

@@ -0,0 +1,237 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the RichenPower Hybrid generators
+*/
+
+#include <AP_Math/AP_Math.h>
+
+#include "SIM_RichenPower.h"
+#include "SITL.h"
+
+#include <stdio.h>
+#include <errno.h>
+
+using namespace SITL;
+
+// table of user settable parameters
+const AP_Param::GroupInfo RichenPower::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: RichenPower Generator sim enable/disable
+    // @Description: Allows you to enable (1) or disable (0) the RichenPower simulator
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE", 0, RichenPower, _enabled, 0),
+
+    // @Param: CTRL_PIN
+    // @DisplayName: Pin RichenPower is connectred to
+    // @Description: The pin number that the RichenPower spinner servo is connected to. (start at 1)
+    // @Range: 0 15
+    // @User: Advanced
+    AP_GROUPINFO("CTRL", 2, RichenPower, _ctrl_pin, -1),
+
+    AP_GROUPEND
+};
+
+RichenPower::RichenPower() : SerialDevice::SerialDevice()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+
+    u.packet.magic1 = 0xAA;
+    u.packet.magic2 = 0x55;
+
+    u.packet.version_major = 0x0A;
+    u.packet.version_minor = 0x00;
+
+    u.packet.footermagic1 = 0x55;
+    u.packet.footermagic2 = 0xAA;
+}
+
+void RichenPower::update(const struct sitl_input &input)
+{
+    if (!_enabled.get()) {
+        return;
+    }
+    update_control_pin(input);
+    update_send();
+}
+
+void RichenPower::update_control_pin(const struct sitl_input &input)
+{
+    const uint32_t now = AP_HAL::millis();
+
+    static const uint16_t INPUT_SERVO_PWM_STOP = 1200;
+    static const uint16_t INPUT_SERVO_PWM_IDLE = 1500;
+    // static const uint16_t INPUT_SERVO_PWM_RUN = 1900;
+
+    // RICHENPOWER, 13:47
+    // 1100~1300 for engine stop, 1300~1800 for idle, and 1800~2000 for run
+
+    const uint16_t control_pwm = _ctrl_pin >= 1 ? input.servos[_ctrl_pin-1] : -1;
+    if (_state != State::STOPPING) {
+        if (control_pwm <= INPUT_SERVO_PWM_STOP && _state != State::STOP) {
+            if (stop_start_ms == 0) {
+                stop_start_ms = now;
+            }
+        } else {
+            stop_start_ms = 0;
+        }
+    }
+    // ::fprintf(stderr, "stop_start_ms=%u\n", stop_start_ms);
+    State newstate;
+    if (control_pwm <= INPUT_SERVO_PWM_STOP) {
+        // stop
+        if (stop_start_ms == 0 || now - stop_start_ms > 30000) {
+            newstate = State::STOP;
+        } else {
+            newstate = State::STOPPING;
+        }
+    } else if (control_pwm <= INPUT_SERVO_PWM_IDLE) {
+        newstate = State::IDLE;
+    } else {
+        newstate = State::RUN;
+    }
+    if (newstate != _state) {
+        set_run_state(newstate);
+    } else {
+        if (_state == State::STOP) {
+            // pass
+        } else {
+            _runtime_ms += now - _last_runtime_ms;
+        }
+        _last_runtime_ms = now;
+    }
+
+    // RICHENPOWER, 13:49
+    // Idle RMP 4800 +-300, RUN RPM 13000 +- 1500
+
+    uint16_t desired_rpm = 0;
+    switch (_state) {
+    case State::STOP:
+        desired_rpm = 0;
+        break;
+    case State::IDLE:
+    case State::STOPPING:
+        desired_rpm = 4800; // +/- 300
+        break;
+    case State::RUN:
+        desired_rpm = 13000; // +/- 1500
+        break;
+    }
+
+    _current_current = AP::sitl()->state.battery_current;
+    _current_current = MIN(_current_current, max_current);
+    if (_current_current > 1 && _state != State::RUN) {
+        AP_HAL::panic("Generator stalled due to high current demand");
+    } else if (_current_current > max_current) {
+        AP_HAL::panic("Generator stalled due to high current demand (run)");
+    }
+
+    // linear degradation in RPM up to maximum load
+    if (desired_rpm) {
+        desired_rpm -= 1500 * (_current_current/max_current);
+    }
+
+    const float max_slew_rpm_per_second = 2000;
+    const float max_slew_rpm = max_slew_rpm_per_second * ((now - last_rpm_update_ms) / 1000.0f);
+    last_rpm_update_ms = now;
+    const float rpm_delta = _current_rpm - desired_rpm;
+    if (rpm_delta > 0) {
+        _current_rpm -= MIN(max_slew_rpm, rpm_delta);
+    } else {
+        _current_rpm += MIN(max_slew_rpm, abs(rpm_delta));
+    }
+
+    // if (!is_zero(rpm_delta)) {
+        // ::fprintf(stderr, "richenpower pwm: %f\n", _current_rpm);
+    // }
+
+}
+
+void RichenPower::RichenUnion::update_checksum()
+{
+    packet.checksum = 0;
+    for (uint8_t i=1; i<6; i++) {
+        packet.checksum += htobe16(checksum_buffer[i]);
+    }
+    packet.checksum = htobe16(packet.checksum);
+}
+
+void RichenPower::update_send()
+{
+    // just send a chunk of data at 1Hz:
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_sent_ms < 1000) {
+        return;
+    }
+    last_sent_ms = now;
+
+    u.packet.rpm = htobe16(_current_rpm);
+    uint32_t runtime_seconds_remainder = _runtime_ms / 1000;
+    u.packet.runtime_hours = runtime_seconds_remainder / 3600;
+    runtime_seconds_remainder %= 3600;
+    u.packet.runtime_minutes = runtime_seconds_remainder / 60;
+    runtime_seconds_remainder %= 60;
+    u.packet.runtime_seconds = runtime_seconds_remainder;
+
+    u.packet.runtime_seconds = runtime_seconds_remainder;
+
+    const int32_t seconds_until_maintenance = (original_seconds_until_maintenance - _runtime_ms/1000.0f);
+    if (seconds_until_maintenance <= 0) {
+        u.packet.seconds_until_maintenance = htobe32(0);
+    } else {
+        u.packet.seconds_until_maintenance = htobe32(seconds_until_maintenance);
+    }
+
+    switch (_state) {
+    case State::IDLE:
+    case State::RUN:
+        u.packet.output_current = htobe16(_current_current * 100);
+        // +/- 3V, depending on draw
+        u.packet.output_voltage = htobe16(100*base_supply_voltage - 3 * (_current_current / max_current));
+        break;
+    case State::STOP:
+    default:
+        u.packet.output_current = 0;
+        u.packet.output_voltage = 0;
+        break;
+    }
+
+    enum class Mode {
+        IDLE = 0,
+        RUN = 1,
+        CHARGE = 2,
+        BALANCE = 3,
+    };
+    if (_state == State::STOP) {
+        u.packet.mode = (uint8_t)Mode::IDLE;
+    } else {
+        u.packet.mode = (uint8_t)Mode::RUN;
+    }
+
+    u.update_checksum();
+
+    // const uint8_t data[] = {
+    //     0xAA, 0x55, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x1E, 0xB0, 0x00, 0x10, 0x00, 0x00, 0x23, 0x7A, 0x23,
+    //     0x7A, 0x11, 0x1D, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    //     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    //     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1E, 0xBE, 0x55, 0xAA,
+    // };
+
+    if (write_to_autopilot((char*)u.parse_buffer, ARRAY_SIZE(u.parse_buffer)) != ARRAY_SIZE(u.parse_buffer)) {
+        AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno));
+    }
+}

libraries/SITL/SIM_RichenPower.h

@@ -0,0 +1,140 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the RichenPower Hybrid generators
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:richenpower --speedup=1
+
+param set SERIAL5_PROTOCOL 30
+param set SERIAL5_BAUD 9600
+param set SIM_RICH_ENABLE 1
+param set SERVO8_FUNCTION 42
+param fetch
+param set SIM_RICH_CTRL 8
+param set RC9_OPTION 85
+
+reboot
+
+arm throttle (denied because generator not running)
+
+./Tools/autotest/autotest.py --gdb --debug build.ArduCopter fly.ArduCopter.RichenPower
+
+*/
+
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+
+#include "SITL_Input.h"
+
+#include "SIM_SerialDevice.h"
+
+#include <stdio.h>
+
+namespace SITL {
+
+class RichenPower : public SerialDevice {
+public:
+
+    RichenPower();
+
+    // update state
+    void update(const struct sitl_input &input);
+
+    static const AP_Param::GroupInfo var_info[];
+
+private:
+
+    // this is what the generator supplies when running full-tilt but
+    // with no load
+    const float base_supply_voltage = 50.0f;
+    const float max_current = 50.0f;
+    const uint32_t original_seconds_until_maintenance = 5*60; // 5 minutes
+
+// They have been... we know that the voltage drops to mid 46v
+// typically if gennie stops
+// So we set batt fs high 46s
+// Gennie keeps batts charged to 49v + typically
+
+    class SITL *_sitl;
+
+    uint32_t last_sent_ms;
+
+    void update_control_pin(const struct sitl_input &input);
+    void update_send();
+
+    enum class State {
+        STOP = 21,
+        IDLE = 22,
+        RUN = 23,
+        STOPPING = 24, // idle cool-down period
+    };
+    State _state = State::STOP;
+    void set_run_state(State newstate) {
+        ::fprintf(stderr, "Moving to state %u from %u\n", (unsigned)newstate, (unsigned)_state);
+        _state = newstate;
+    }
+
+    AP_Int8  _enabled;  // enable richenpower sim
+    AP_Int8  _ctrl_pin;
+
+    float _current_rpm;
+    uint32_t _runtime_ms;
+    uint32_t _last_runtime_ms;
+
+    float _current_current;
+
+    uint32_t last_rpm_update_ms;
+
+    // packet to send:
+    struct PACKED RichenPacket {
+        uint8_t magic1;
+        uint8_t magic2;
+        uint8_t version_minor;
+        uint8_t version_major;
+        uint8_t runtime_seconds;
+        uint8_t runtime_minutes;
+        uint16_t runtime_hours;
+        uint32_t seconds_until_maintenance;
+        uint16_t errors;
+        uint16_t rpm;
+        uint16_t throttle;
+        uint16_t idle_throttle;
+        uint16_t output_voltage;
+        uint16_t output_current;
+        uint16_t dynamo_current;
+        uint8_t mode;
+        uint8_t unknown1;
+        uint8_t unknown6[38]; // "data"?!
+        uint16_t checksum;
+        uint8_t footermagic1;
+        uint8_t footermagic2;
+    };
+    assert_storage_size<RichenPacket, 70> _assert_storage_size_RichenPacket;
+
+    union RichenUnion {
+        uint8_t parse_buffer[70];
+        uint16_t checksum_buffer[35];
+        struct RichenPacket packet;
+
+        void update_checksum();
+    };
+    RichenUnion u;
+
+    // time we were asked to stop; 
+    uint32_t stop_start_ms;
+};
+
+}

libraries/SITL/SIM_SerialDevice.cpp

@@ -16,7 +16,11 @@
   base class for serially-attached simulated devices
 */
 
+#include <AP_HAL/AP_HAL.h>
+#include <SITL/SITL.h>
+
 #include "SIM_SerialDevice.h"
+
 #include <stdio.h>
 #include <unistd.h>
 #include <fcntl.h>

libraries/SITL/SIM_SerialDevice.h

@@ -18,9 +18,7 @@
 
 #pragma once
 
-#include <SITL/SITL.h>
+#include <unistd.h>
-
-#include <AP_HAL/utility/RingBuffer.h>
 
 namespace SITL {
 
@@ -40,7 +38,7 @@ public:
 
 protected:
 
-    SITL *_sitl;
+    class SITL *_sitl;
 
     int fd_their_end;
     int fd_my_end;

libraries/SITL/SITL.cpp

@@ -273,6 +273,9 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     AP_GROUPINFO("MAG3_ORIENT",  30, SITL,  mag_orient[2], 0),
 #endif
 
+    // @Path: ./SIM_RichenPower.cpp
+    AP_SUBGROUPINFO(richenpower_sim, "RICH_", 31, SITL, RichenPower),
+
     AP_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -16,6 +16,7 @@
 #include "SIM_Sprayer.h"
 #include "SIM_ToneAlarm.h"
 #include "SIM_EFI_MegaSquirt.h"
+#include "SIM_RichenPower.h"
 
 namespace SITL {
 
@@ -351,6 +352,7 @@ public:
     Buzzer buzzer_sim;
     ToneAlarm tonealarm_sim;
     SIM_Precland precland_sim;
+    RichenPower richenpower_sim;
 
     struct {
         // LED state, for serial LED emulation