mirror of https://github.com/ArduPilot/ardupilot
254 lines
7.6 KiB
C++
254 lines
7.6 KiB
C++
/*
|
|
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 <AP_HAL/utility/sparse-endian.h>
|
|
|
|
#include <stdio.h>
|
|
#include <errno.h>
|
|
|
|
using namespace SITL;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
// 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::set_run_state(State newstate) {
|
|
hal.console->printf("Moving to state %u from %u\n", (unsigned)newstate, (unsigned)_state);
|
|
_state = newstate;
|
|
}
|
|
|
|
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,
|
|
OFF = 4,
|
|
};
|
|
switch (_state) {
|
|
case State::STOP:
|
|
u.packet.mode = (uint8_t)Mode::OFF;
|
|
break;
|
|
case State::STOPPING:
|
|
case State::IDLE:
|
|
u.packet.mode = (uint8_t)Mode::IDLE;
|
|
break;
|
|
case State::RUN:
|
|
u.packet.mode = (uint8_t)Mode::RUN;
|
|
break;
|
|
}
|
|
|
|
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));
|
|
}
|
|
}
|