From 88c8dd84656e9321a83148ed484b33f59e9e7cf7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 28 Sep 2019 21:32:57 +1000 Subject: [PATCH] SITL: add RichenPower generator simulator --- libraries/SITL/SIM_Aircraft.cpp | 5 + libraries/SITL/SIM_Aircraft.h | 3 + libraries/SITL/SIM_RichenPower.cpp | 237 ++++++++++++++++++++++++++++ libraries/SITL/SIM_RichenPower.h | 140 ++++++++++++++++ libraries/SITL/SIM_SerialDevice.cpp | 4 + libraries/SITL/SIM_SerialDevice.h | 6 +- libraries/SITL/SITL.cpp | 3 + libraries/SITL/SITL.h | 2 + 8 files changed, 396 insertions(+), 4 deletions(-) create mode 100644 libraries/SITL/SIM_RichenPower.cpp create mode 100644 libraries/SITL/SIM_RichenPower.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index f93af97747..b95dadffef 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/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) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index c086b6e6e4..f50a85a8a5 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/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 @@ -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; }; diff --git a/libraries/SITL/SIM_RichenPower.cpp b/libraries/SITL/SIM_RichenPower.cpp new file mode 100644 index 0000000000..da388f49a3 --- /dev/null +++ b/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 . + */ +/* + Simulator for the RichenPower Hybrid generators +*/ + +#include + +#include "SIM_RichenPower.h" +#include "SITL.h" + +#include +#include + +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)); + } +} diff --git a/libraries/SITL/SIM_RichenPower.h b/libraries/SITL/SIM_RichenPower.h new file mode 100644 index 0000000000..9c73e0c35d --- /dev/null +++ b/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 . + */ +/* + 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 + +#include "SITL_Input.h" + +#include "SIM_SerialDevice.h" + +#include + +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 _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; +}; + +} diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp index 7da982c37a..3efd822326 100644 --- a/libraries/SITL/SIM_SerialDevice.cpp +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -16,7 +16,11 @@ base class for serially-attached simulated devices */ +#include +#include + #include "SIM_SerialDevice.h" + #include #include #include diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index ed166d6566..8492836ad1 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -18,9 +18,7 @@ #pragma once -#include - -#include +#include namespace SITL { @@ -40,7 +38,7 @@ public: protected: - SITL *_sitl; + class SITL *_sitl; int fd_their_end; int fd_my_end; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 0a46e55dab..12813224b3 100644 --- a/libraries/SITL/SITL.cpp +++ b/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 }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index de9a6a367c..c6d6324007 100644 --- a/libraries/SITL/SITL.h +++ b/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