AP_Generator: add serial driver to read from RichenPower hybrid generators

AP_RichenPower: add support for sending HYBRID message

libraries/AP_Generator/AP_Generator_RichenPower.cpp

@@ -0,0 +1,440 @@
+/*
+   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/>.
+ */
+
+#include "AP_Generator_RichenPower.h"
+
+#if GENERATOR_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Logger/AP_Logger.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <GCS_MAVLink/GCS.h>
+
+#include <AP_HAL/utility/sparse-endian.h>
+
+extern const AP_HAL::HAL& hal;
+
+// TODO: failsafe if we don't get readings?
+
+AP_Generator_RichenPower::AP_Generator_RichenPower()
+{
+    if (_singleton) {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        AP_HAL::panic("Too many richenpower generators");
+#endif
+        return;
+    }
+    _singleton = this;
+}
+
+void AP_Generator_RichenPower::init()
+{
+    const AP_SerialManager &serial_manager = AP::serialmanager();
+
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Generator, 0);
+    if (uart != nullptr) {
+        const uint32_t baud = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Generator, 0);
+        uart->begin(baud, 256, 256);
+    }
+}
+
+void AP_Generator_RichenPower::move_header_in_buffer(uint8_t initial_offset)
+{
+    uint8_t header_offset;
+    for (header_offset=initial_offset; header_offset<body_length; header_offset++) {
+        if (u.parse_buffer[header_offset] == HEADER_MAGIC1) {
+            break;
+        }
+    }
+    if (header_offset != 0) {
+        // header was found, but not at index 0; move it back to start of array
+        memmove(u.parse_buffer, &u.parse_buffer[header_offset], body_length - header_offset);
+        body_length -= header_offset;
+    }
+}
+
+// read - read serial port, return true if a new reading has been found
+bool AP_Generator_RichenPower::get_reading()
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // Example of a packet from a H2 controller:
+    //AA 55 00 0A 00 00 00 00 00 04 1E B0 00 10 00 00 23 7A 23
+    //7A 11 1D 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00
+    //00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
+    //00 00 00 00 00 00 00 00 00 1E BE 55 AA
+    //16 bit one data Hight + Low
+
+    // fill our buffer some more:
+    uint32_t nbytes = uart->read(&u.parse_buffer[body_length],
+                                 ARRAY_SIZE(u.parse_buffer)-body_length);
+    if (nbytes == 0) {
+        return false;
+    }
+    body_length += nbytes;
+
+    move_header_in_buffer(0);
+
+    // header byte 1 is correct.
+    if (body_length < ARRAY_SIZE(u.parse_buffer)) {
+        // need a full buffer to have a valid message...
+        return false;
+    }
+
+    if (u.packet.headermagic2 != HEADER_MAGIC2) {
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    // check for the footer signature:
+    if (u.packet.footermagic1 != FOOTER_MAGIC1) {
+        move_header_in_buffer(1);
+        return false;
+    }
+    if (u.packet.footermagic2 != FOOTER_MAGIC2) {
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    // calculate checksum....
+    uint16_t checksum = 0;
+    for (uint8_t i=0; i<5; i++) {
+        checksum += be16toh(checksum_buffer[i]);
+    }
+
+    if (checksum != be16toh(u.packet.checksum)) {
+        move_header_in_buffer(1);
+        return false;
+    }
+
+    // check the version:
+    const uint16_t version = be16toh(u.packet.version);
+    const uint8_t major = version / 100;
+    const uint8_t minor = (version % 100) / 10;
+    const uint8_t point = version % 10;
+    if (!protocol_information_anounced) {
+        gcs().send_text(MAV_SEVERITY_INFO, "RichenPower: protocol %u.%u.%u", major, minor, point);
+        protocol_information_anounced = true;
+    }
+
+    last_reading.runtime = be16toh(u.packet.runtime_hours) * 3600 +
+        u.packet.runtime_minutes * 60 +
+        u.packet.runtime_seconds;
+    last_reading.seconds_until_maintenance = be32toh(u.packet.seconds_until_maintenance);
+    last_reading.errors = be16toh(u.packet.errors);
+    last_reading.rpm = be16toh(u.packet.rpm);
+    last_reading.output_voltage = be16toh(u.packet.output_voltage) / 100.0f;
+    last_reading.output_current = be16toh(u.packet.output_current) / 100.0f;
+    last_reading.mode = (Mode)u.packet.mode;
+
+    last_reading_ms = AP_HAL::millis();
+
+    body_length = 0;
+
+    return true;
+}
+
+void AP_Generator_RichenPower::update_heat()
+{
+    // assume heat increase is directly proportional to RPM.
+    const uint32_t now = AP_HAL::millis();
+    uint16_t rpm = last_reading.rpm;
+    if (now - last_reading_ms > 2000) {
+        // if we're not getting updates, assume we're getting colder
+        rpm = 0;
+        // ... and resend the version information when we get something again
+        protocol_information_anounced = false;
+    }
+
+    const uint32_t time_delta_ms = now - last_heat_update_ms;
+    last_heat_update_ms = now;
+
+    heat += rpm * time_delta_ms * (1/1000.0f);
+    // cap the heat of the motor:
+    heat = MIN(heat, 60 * RUN_RPM); // so cap heat at 60 seconds at run-speed
+    // now lose some heat to the environment
+    heat -= (heat * environment_loss_factor * (time_delta_ms * (1/1000.0f)));  // lose some % of heat per second
+}
+
+constexpr float AP_Generator_RichenPower::heat_required_for_run()
+{
+    // assume that heat is proportional to RPM.  Return a number
+    // proportial to RPM.  Reduce it to account for the cooling some%/s
+    // cooling
+    return (30 * IDLE_RPM) * environment_loss_30s;
+}
+bool AP_Generator_RichenPower::generator_ok_to_run() const
+{
+    return heat > heat_required_for_run();
+}
+
+constexpr float AP_Generator_RichenPower::heat_required_for_supply()
+{
+    // account for cooling that happens in that 60 seconds
+    return (30 * IDLE_RPM + 30 * RUN_RPM) * environment_loss_60s;
+}
+bool AP_Generator_RichenPower::generator_ok_to_supply() const
+{
+    // duplicated into prearms
+    return heat > heat_required_for_supply();
+}
+
+
+
+/*
+  update the state of the sensor
+*/
+void AP_Generator_RichenPower::update(void)
+{
+    if (uart == nullptr) {
+        return;
+    }
+
+    update_servo_channel();
+
+    update_runstate();
+
+    (void)get_reading();
+
+    update_heat();
+
+    Log_Write();
+}
+
+void AP_Generator_RichenPower::update_runstate()
+{
+    if (_servo_channel == nullptr) {
+        // doesn't really matter what we want to do; we're not going
+        // to be able to affect it...
+        return;
+    }
+
+    // don't run the generator while the safety is on:
+    // if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+    //     _servo_channel->set_output_pwm(SERVO_PWM_STOP);
+    //     return;
+    // }
+
+    static const uint16_t SERVO_PWM_STOP = 1200;
+    static const uint16_t SERVO_PWM_IDLE = 1500;
+    static const uint16_t SERVO_PWM_RUN = 1900;
+
+    switch (runstate) {
+    case RunState::STOP:
+        _servo_channel->set_output_pwm(SERVO_PWM_STOP);
+        break;
+    case RunState::IDLE:
+        _servo_channel->set_output_pwm(SERVO_PWM_IDLE);
+        break;
+    case RunState::RUN:
+        // we must have
+        if (!generator_ok_to_run()) {
+            _servo_channel->set_output_pwm(SERVO_PWM_IDLE);
+            break;
+        }
+        _servo_channel->set_output_pwm(SERVO_PWM_RUN);
+        break;
+    }
+}
+
+void AP_Generator_RichenPower::update_servo_channel(void)
+{
+    const uint32_t now = AP_HAL::millis();
+    if (now - _last_servo_channel_check < 1000) {
+        return;
+    }
+    _last_servo_channel_check = now;
+    SRV_Channel *control = SRV_Channels::get_channel_for(SRV_Channel::k_richenpower_control);
+    if (control == nullptr) {
+        if (_servo_channel != nullptr) {
+            gcs().send_text(MAV_SEVERITY_INFO, "RichenPower: no control channel");
+            _servo_channel = nullptr;
+        }
+        return;
+    }
+    if (_servo_channel != nullptr &&
+        _servo_channel->get_function() == SRV_Channel::k_richenpower_control) {
+        // note that _servo_channel could actually be == control here
+        return;
+    }
+    // gcs().send_text(MAV_SEVERITY_INFO, "RP: using control channel");
+    _servo_channel = control;
+}
+
+void AP_Generator_RichenPower::Log_Write()
+{
+    if (last_logged_reading_ms == last_reading_ms) {
+        return;
+    }
+    last_logged_reading_ms = last_reading_ms;
+
+    AP::logger().Write(
+        "GEN",
+        "TimeUS,runTime,maintTime,errors,rpm,ovolt,ocurr,mode",
+        "s-------",
+        "F-------",
+        "QIIHHffB",
+        AP_HAL::micros64(),
+        last_reading.runtime,
+        last_reading.seconds_until_maintenance,
+        last_reading.errors,
+        last_reading.rpm,
+        last_reading.output_voltage,
+        last_reading.output_current,
+        last_reading.mode
+        );
+}
+
+bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
+{
+    if (uart == nullptr) {
+        // not configured in serial manager
+        return true;
+    }
+    const uint32_t now = AP_HAL::millis();
+
+    if (now - last_reading_ms > 2000) { // we expect @1Hz
+        snprintf(failmsg, failmsg_len, "no messages in %ums", unsigned(now - last_reading_ms));
+        return false;
+    }
+    if (last_reading.seconds_until_maintenance == 0) {
+        snprintf(failmsg, failmsg_len, "requires maintenance");
+        return false;
+    }
+    if (last_reading.errors) {
+        for (uint16_t i=0; i<16; i++) {
+            if (last_reading.errors & (1U << (uint16_t)i)) {
+                if (i < (uint16_t)Errors::LAST) {
+                    snprintf(failmsg, failmsg_len, "error: %s", error_strings[i]);
+                } else {
+                    snprintf(failmsg, failmsg_len, "unknown error: 1U<<%u", i);
+                }
+            }
+        }
+        return false;
+    }
+    if (last_reading.mode != Mode::RUN && last_reading.mode != Mode::CHARGE && last_reading.mode != Mode::BALANCE) {
+        snprintf(failmsg, failmsg_len, "not running");
+        return false;
+    }
+    if (runstate != RunState::RUN) {
+        snprintf(failmsg, failmsg_len, "requested state is not RUN");
+        return false;
+    }
+    if (!generator_ok_to_supply()) {
+        snprintf(failmsg, failmsg_len, "warming up (%.0f%%)", (heat *100 / heat_required_for_supply()));
+        return false;
+    }
+
+    return true;
+}
+
+bool AP_Generator_RichenPower::voltage(float &v) const
+{
+    if (last_reading_ms == 0) {
+        return false;
+    }
+    v = last_reading.output_voltage;
+    return true;
+}
+
+bool AP_Generator_RichenPower::current(float &curr) const
+{
+    if (last_reading_ms == 0) {
+        return false;
+    }
+    curr = last_reading.output_current;
+    return true;
+}
+
+bool AP_Generator_RichenPower::healthy() const
+{
+    const uint32_t now = AP_HAL::millis();
+
+    if (last_reading_ms == 0 || now - last_reading_ms > 2000) {
+        return false;
+    }
+    if (last_reading.errors) {
+        return false;
+    }
+    return true;
+}
+
+//send generator status
+void AP_Generator_RichenPower::send_generator_status(const GCS_MAVLINK &channel)
+{
+    uint64_t status = 0;
+    switch (last_reading.mode) {
+    case Mode::IDLE:
+        break;
+    case Mode::RUN:
+        status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
+        break;
+    case Mode::CHARGE:
+        status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
+        status |= MAV_GENERATOR_STATUS_FLAG_CHARGING;
+        break;
+    case Mode::BALANCE:
+        status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
+        status |= MAV_GENERATOR_STATUS_FLAG_CHARGING;
+        break;
+    }
+
+    if (last_reading.rpm == 0) {
+        status |= MAV_GENERATOR_STATUS_FLAG_OFF;
+    }
+
+    if (last_reading.errors & (uint8_t)Errors::Overload) {
+        status |= MAV_GENERATOR_STATUS_FLAG_OVERCURRENT_FAULT;
+    }
+    if (last_reading.errors & (uint8_t)Errors::LowVoltageOutput) {
+        status |= MAV_GENERATOR_STATUS_FLAG_REDUCED_POWER;
+    }
+
+    mavlink_msg_generator_status_send(
+        channel.get_chan(),
+        status,
+        last_reading.rpm, // generator_speed
+        std::numeric_limits<double>::quiet_NaN(), // battery_current; current into/out of battery
+        last_reading.output_current, // load_current; Current going to UAV
+        std::numeric_limits<double>::quiet_NaN(), // power_generated; the power being generated
+        last_reading.output_voltage, // bus_voltage; Voltage of the bus seen at the generator
+        INT16_MAX, // rectifier_temperature
+        std::numeric_limits<double>::quiet_NaN(), // bat_current_setpoint; The target battery current
+        INT16_MAX // generator temperature
+        );
+}
+
+/*
+ * Get the AP_Generator singleton
+ */
+AP_Generator_RichenPower *AP_Generator_RichenPower::_singleton;
+AP_Generator_RichenPower *AP_Generator_RichenPower::get_singleton()
+{
+    return _singleton;
+}
+
+namespace AP {
+
+AP_Generator_RichenPower *generator()
+{
+    return AP_Generator_RichenPower::get_singleton();
+}
+
+};
+
+#endif

libraries/AP_Generator/AP_Generator_RichenPower.h

@@ -0,0 +1,202 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+#include <SRV_Channel/SRV_Channel.h>
+
+#include <stdint.h>
+#include <stdio.h>
+
+#ifndef GENERATOR_ENABLED
+#define GENERATOR_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+#if GENERATOR_ENABLED
+
+/*
+ *  Example setup:
+ *  param set SERIAL2_PROTOCOL 30
+ *  param set SERIAL2_BAUD 9600
+ *  param set RC9_OPTION 85
+ *  param set SERVO8_FUNCTION 42
+ */
+
+class AP_Generator_RichenPower
+{
+
+public:
+
+    AP_Generator_RichenPower();
+
+    /* Do not allow copies */
+    AP_Generator_RichenPower(const AP_Generator_RichenPower &other) = delete;
+    AP_Generator_RichenPower &operator=(const AP_Generator_RichenPower&) = delete;
+
+    static AP_Generator_RichenPower *get_singleton();
+
+    void init();
+    void update(void);
+
+    void stop() { set_runstate(RunState::STOP); }
+    void idle() { set_runstate(RunState::IDLE); }
+    void run() { set_runstate(RunState::RUN); }
+
+    void send_generator_status(const GCS_MAVLINK &channel);
+
+    bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const;
+
+    // these return false if a reading is not available.  They do not
+    // modify the passed-in value if they return false.
+    bool voltage(float &voltage) const;
+    bool current(float &current) const;
+
+    bool healthy() const;
+
+private:
+
+    static AP_Generator_RichenPower *_singleton;
+
+    bool get_reading();
+    AP_HAL::UARTDriver *uart = nullptr;
+
+    void update_servo_channel();
+    uint32_t _last_servo_channel_check;
+    void update_rc_channel();
+    uint32_t _last_rc_channel_check;
+
+    enum class RunState {
+        STOP = 17,
+        IDLE = 18,
+        RUN = 19,
+    };
+    RunState runstate = RunState::STOP;
+    void set_runstate(RunState newstate) {
+        // gcs().send_text(MAV_SEVERITY_INFO, "RichenPower: Moving to state (%u) from (%u)\n", (unsigned)newstate, (unsigned)runstate);
+        runstate = newstate;
+    }
+    void update_runstate();
+
+    bool protocol_information_anounced;
+
+    enum class Mode {
+        IDLE = 0,
+        RUN = 1,
+        CHARGE = 2,
+        BALANCE = 3,
+    };
+
+    struct Reading {
+        uint32_t    runtime; //seconds
+        uint32_t    seconds_until_maintenance;
+        uint16_t    errors;
+        uint16_t    rpm;
+        float       output_voltage;
+        float       output_current;
+        Mode        mode;
+    };
+
+    void Log_Write();
+
+    struct Reading last_reading;
+    uint32_t last_reading_ms;
+
+    const uint8_t HEADER_MAGIC1 = 0xAA;
+    const uint8_t HEADER_MAGIC2 = 0x55;
+
+    const uint8_t FOOTER_MAGIC1 = 0x55;
+    const uint8_t FOOTER_MAGIC2 = 0xAA;
+
+    enum class Errors { // bitmask
+        MaintenanceRequired = 0,
+        StartDisabled = 1,
+        Overload = 2,
+        LowVoltageOutput = 3,
+        LowBatteryVoltage = 4,
+        LAST
+    };
+
+    const char *error_strings[5] = {
+        "MaintenanceRequired",
+        "StartDisabled",
+        "Overload",
+        "LowVoltageOutput",
+        "LowBatteryVoltage",
+    };
+    static_assert(ARRAY_SIZE(error_strings) == (uint8_t)Errors::LAST,
+                  "have error string for each error");
+
+    struct PACKED RichenPacket {
+        uint8_t headermagic1;
+        uint8_t headermagic2;
+        uint16_t version;
+        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];
+        struct RichenPacket packet;
+    };
+    RichenUnion u;
+    uint16_t *checksum_buffer = (uint16_t*)&u.parse_buffer[2];
+
+    uint8_t body_length;
+
+    // move the expected header bytes into &buffer[0], adjusting
+    // body_length as appropriate.
+    void move_header_in_buffer(uint8_t initial_offset);
+
+    // servo channel used to control the generator:
+    SRV_Channel *_servo_channel;
+
+    // RC input generator for pilot to specify desired generator state
+    RC_Channel *_rc_channel;
+
+    // a simple heat model to avoid the motor moving to run too fast
+    // or being stopped before cooldown.
+    uint32_t last_heat_print;
+    uint32_t last_heat_update_ms;
+    float heat;
+    void update_heat();
+
+    bool generator_ok_to_run() const;
+    bool generator_ok_to_supply() const;
+    static constexpr float heat_required_for_supply();
+    static constexpr float heat_required_for_run();
+
+    static const uint16_t RUN_RPM = 15000;
+    static const uint16_t IDLE_RPM = 4800;
+
+    static constexpr float environment_loss_factor = 0.005f;
+    // powf is not constexpr, so we create a const for it:
+    // powf(1.0f-environment_loss_factor, 30)
+    static constexpr float environment_loss_30s = 0.860384;
+    static constexpr float environment_loss_60s = 0.740261;
+
+    // logging state
+    uint32_t last_logged_reading_ms;
+};
+
+namespace AP {
+    AP_Generator_RichenPower *generator();
+};
+
+#endif