AP_Generator: update prearm checks, fill new GENERATOR_STATUS fields

AP_Generator_Richenpower: only update servo channel while disarmed

AP_Generator_Richenpower: move warning for servo output channel to prearm checks

AP_Generator: state is simply off if RPM is zero

AP_Generator: send runtime and seconds-until-maintenance in GENERATOR_STATUS

AP_Generator_RichenPower: correct runtime seconds/minutes position in packets

AP_Generator_RichenPower: correct and expand use of mode packet entry

AP_Generator_RichenPower: fail prearm check if generator not seen

AP_Generator_RichenPower: make prearm check for needing maintenance  warn-only

AP_Generator_RichenPower: rename runstate to pilot_desired_runstate

AP_Generator_RichenPower: tweak decoding of time-to-maintenance

AP_Generator: add hysteresis for warmup/cooldown in IDLE

AP_Generator: stop generator in case of vehicle crash

AP_Generator: generator must go through idle from run to stop

AP_Generator: emit statustext when generator configured but not present

AP_Generator: do not log if LOG_BITMASK is zero

AP_Generator: do not warn user if generator configured but not present

They won't be able to start the generator if we can't see it.

Don't update the runstate if no readings seen.

AP_Generator: do not send generator status if no readings seen

libraries/AP_Generator/AP_Generator_RichenPower.cpp

@@ -21,13 +21,13 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 
 #include <AP_HAL/utility/sparse-endian.h>
 
 extern const AP_HAL::HAL& hal;
 
-// TODO: failsafe if we don't get readings?
-
+// Generator constructor; only one generator is curently permitted
 AP_Generator_RichenPower::AP_Generator_RichenPower()
 {
     if (_singleton) {
@@ -39,6 +39,7 @@ AP_Generator_RichenPower::AP_Generator_RichenPower()
     _singleton = this;
 }
 
+// init method; configure communications with the generator
 void AP_Generator_RichenPower::init()
 {
     const AP_SerialManager &serial_manager = AP::serialmanager();
@@ -50,6 +51,10 @@ void AP_Generator_RichenPower::init()
     }
 }
 
+
+// find a RichenPower message in the buffer, starting at
+// initial_offset.  If dound, that message (or partial message) will
+// be moved to the start of the buffer.
 void AP_Generator_RichenPower::move_header_in_buffer(uint8_t initial_offset)
 {
     uint8_t header_offset;
@@ -68,10 +73,6 @@ void AP_Generator_RichenPower::move_header_in_buffer(uint8_t initial_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
@@ -135,7 +136,7 @@ bool AP_Generator_RichenPower::get_reading()
     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.seconds_until_maintenance = be16toh(u.packet.seconds_until_maintenance_high) * 65536 + be16toh(u.packet.seconds_until_maintenance_low);
     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;
@@ -146,9 +147,63 @@ bool AP_Generator_RichenPower::get_reading()
 
     body_length = 0;
 
+    // update the time we started idling at:
+    if (last_reading.mode == Mode::IDLE) {
+        if (idle_state_start_ms == 0) {
+            idle_state_start_ms = last_reading_ms;
+        }
+    } else {
+        idle_state_start_ms = 0;
+    }
+
     return true;
 }
 
+// update the synthetic heat measurement we are keeping for the
+// generator.  We keep a synthetic heat measurement as the telemetry
+// from the unit does not include the motor temperature, so we
+// approximate one by assuming it produces heat in proportion to its
+// current RPM.
+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 * heat_environment_loss_factor * (time_delta_ms * (1/1000.0f)));  // lose some % of heat per second
+}
+
+// returns true if the generator should be allowed to move into
+// the "run" (high-RPM) state:
+bool AP_Generator_RichenPower::generator_ok_to_run() const
+{
+    return heat > heat_required_for_run();
+}
+
+// returns an amount of synthetic heat required for the generator
+// to move into the "run" state:
+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 (45 * IDLE_RPM) * heat_environment_loss_30s;
+}
+
+
 /*
   update the state of the sensor
 */
@@ -158,23 +213,22 @@ void AP_Generator_RichenPower::update(void)
         return;
     }
 
-    update_servo_channel();
-
-    update_runstate();
+    if (last_reading_ms != 0) {
+        update_runstate();
+    }
 
     (void)get_reading();
 
+    update_heat();
+
     Log_Write();
 }
 
+// update_runstate updates the servo output we use to control the
+// generator.  Which state we request the generator move to depends on
+// the RC inputcontrol and the temperature the generator is at.
 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);
@@ -185,45 +239,68 @@ void AP_Generator_RichenPower::update_runstate()
     static const uint16_t SERVO_PWM_IDLE = 1500;
     static const uint16_t SERVO_PWM_RUN = 1900;
 
-    switch (runstate) {
+    // if the vehicle crashes then we assume the pilot wants to stop
+    // the motor.  This is done as a once-off when the crash is
+    // detected to allow the operator to rearm the vehicle, or we end
+    // up in a catch-22 situation where we force the stop state on the
+    // generator so they can't arm and can't start the generator
+    // because the vehicle is crashed.
+    if (AP::vehicle()->is_crashed()) {
+        if (!vehicle_was_crashed) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Crash; stopping generator");
+            pilot_desired_runstate = RunState::STOP;
+            vehicle_was_crashed = true;
+        }
+    } else {
+        vehicle_was_crashed = false;
+    }
+
+    if (commanded_runstate != pilot_desired_runstate &&
+        !hal.util->get_soft_armed()) {
+        // consider changing the commanded runstate to the pilot
+        // desired runstate:
+        commanded_runstate = RunState::IDLE;
+        switch (pilot_desired_runstate) {
+        case RunState::STOP:
+            // The H2 will keep the motor running for ~20 seconds for cool-down
+            // we must go via the idle state or the H2 disallows moving to stop!
+            if (time_in_idle_state_ms() > 1000) {
+                commanded_runstate = pilot_desired_runstate;
+            }
+            break;
+        case RunState::IDLE:
+            // can always switch to idle
+            commanded_runstate = pilot_desired_runstate;
+            break;
+        case RunState::RUN:
+            // must have idled for a while before moving to run:
+            if (generator_ok_to_run()) {
+                commanded_runstate = pilot_desired_runstate;
+            }
+            break;
+        }
+    }
+
+    switch (commanded_runstate) {
     case RunState::STOP:
-        _servo_channel->set_output_pwm(SERVO_PWM_STOP);
+        SRV_Channels::set_output_pwm(SRV_Channel::k_generator_control, SERVO_PWM_STOP);
         break;
     case RunState::IDLE:
-        _servo_channel->set_output_pwm(SERVO_PWM_IDLE);
+        SRV_Channels::set_output_pwm(SRV_Channel::k_generator_control, SERVO_PWM_IDLE);
         break;
     case RunState::RUN:
-        _servo_channel->set_output_pwm(SERVO_PWM_RUN);
+        SRV_Channels::set_output_pwm(SRV_Channel::k_generator_control, SERVO_PWM_RUN);
         break;
     }
 }
 
-void AP_Generator_RichenPower::update_servo_channel(void)
+// log generator status to the onboard log
+void AP_Generator_RichenPower::Log_Write()
 {
-    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;
-        }
+#define MASK_LOG_ANY                    0xFFFF
+    if (!AP::logger().should_log(MASK_LOG_ANY)) {
         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;
     }
@@ -246,6 +323,9 @@ void AP_Generator_RichenPower::Log_Write()
         );
 }
 
+// generator prearm checks; notably, if we never see a generator we do
+// not run the checks.  Generators are attached/detached at will, and
+// reconfiguring is painful.
 bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
 {
     if (uart == nullptr) {
@@ -254,7 +334,6 @@ bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len)
     }
     if (last_reading_ms == 0) {
         // allow optional use of generator
-        snprintf(failmsg, failmsg_len, "no RichenPower generator present");
         return true;
     }
 
@@ -266,8 +345,12 @@ bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len)
     }
     if (last_reading.seconds_until_maintenance == 0) {
         snprintf(failmsg, failmsg_len, "requires maintenance");
+    }
+    if (SRV_Channels::get_channel_for(SRV_Channel::k_generator_control) == nullptr) {
+        snprintf(failmsg, failmsg_len, "need a servo output channel");
         return false;
     }
+
     if (last_reading.errors) {
         for (uint16_t i=0; i<16; i++) {
             if (last_reading.errors & (1U << (uint16_t)i)) {
@@ -280,18 +363,25 @@ bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len)
         }
         return false;
     }
-    if (last_reading.mode != Mode::RUN && last_reading.mode != Mode::CHARGE && last_reading.mode != Mode::BALANCE) {
-        snprintf(failmsg, failmsg_len, "not running");
+    if (pilot_desired_runstate != RunState::RUN) {
+        snprintf(failmsg, failmsg_len, "requested state is not RUN");
         return false;
     }
-    if (runstate != RunState::RUN) {
-        snprintf(failmsg, failmsg_len, "requested state is not RUN");
+    if (commanded_runstate != RunState::RUN) {
+        snprintf(failmsg, failmsg_len, "Generator warming up (%.0f%%)", (heat *100 / heat_required_for_run()));
+        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;
     }
 
     return true;
 }
 
+// voltage returns the last voltage read from the generator telemetry
 bool AP_Generator_RichenPower::voltage(float &v) const
 {
     if (last_reading_ms == 0) {
@@ -301,6 +391,7 @@ bool AP_Generator_RichenPower::voltage(float &v) const
     return true;
 }
 
+// current returns the last current read from the generator telemetry
 bool AP_Generator_RichenPower::current(float &curr) const
 {
     if (last_reading_ms == 0) {
@@ -310,6 +401,8 @@ bool AP_Generator_RichenPower::current(float &curr) const
     return true;
 }
 
+// healthy returns true if the generator is not present, or it is
+// present, providing telemetry and not indicating an errors.
 bool AP_Generator_RichenPower::healthy() const
 {
     const uint32_t now = AP_HAL::millis();
@@ -323,28 +416,41 @@ bool AP_Generator_RichenPower::healthy() const
     return true;
 }
 
-//send generator status
+//send mavlink 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_ms == 0) {
+        // nothing to report
+        return;
     }
 
+    uint64_t status = 0;
     if (last_reading.rpm == 0) {
         status |= MAV_GENERATOR_STATUS_FLAG_OFF;
+    } else {
+        switch (last_reading.mode) {
+        case Mode::OFF:
+            status |= MAV_GENERATOR_STATUS_FLAG_OFF;
+            break;
+        case Mode::IDLE:
+            if (pilot_desired_runstate == RunState::RUN) {
+                status |= MAV_GENERATOR_STATUS_FLAG_WARMING_UP;
+            } else {
+                status |= MAV_GENERATOR_STATUS_FLAG_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.errors & (uint8_t)Errors::Overload) {
@@ -375,8 +481,8 @@ void AP_Generator_RichenPower::send_generator_status(const GCS_MAVLINK &channel)
         INT16_MAX, // rectifier_temperature
         std::numeric_limits<double>::quiet_NaN(), // bat_current_setpoint; The target battery current
         INT16_MAX, // generator temperature
-        UINT32_MAX, // seconds this generator has run since it was rebooted
-        INT32_MAX // seconds until this generator requires maintenance
+        last_reading.runtime,
+        (int32_t)last_reading.seconds_until_maintenance
         );
 }
 

libraries/AP_Generator/AP_Generator_RichenPower.h

@@ -17,10 +17,10 @@
 
 /*
  *  Example setup:
- *  param set SERIAL2_PROTOCOL 30
+ *  param set SERIAL2_PROTOCOL 30  # Generator protocol
  *  param set SERIAL2_BAUD 9600
- *  param set RC9_OPTION 85
- *  param set SERVO8_FUNCTION 42
+ *  param set RC9_OPTION 85        # pilot directive for generator
+ *  param set SERVO8_FUNCTION 42   # autopilot directive to generator
  */
 
 class AP_Generator_RichenPower
@@ -36,15 +36,22 @@ public:
 
     static AP_Generator_RichenPower *get_singleton();
 
+    // init should be called at vehicle startup to get the generator library ready
     void init();
+    // update should be called regulkarly to update the generator state
     void update(void);
 
-    void stop() { set_runstate(RunState::STOP); }
-    void idle() { set_runstate(RunState::IDLE); }
-    void run() { set_runstate(RunState::RUN); }
+    // methods to control the generator state:
+    void stop() { set_pilot_desired_runstate(RunState::STOP); }
+    void idle() { set_pilot_desired_runstate(RunState::IDLE); }
+    void run() { set_pilot_desired_runstate(RunState::RUN); }
 
+    // method to send a GENERATOR_STATUS mavlink message
     void send_generator_status(const GCS_MAVLINK &channel);
 
+    // prearm checks to ensure generator is good for arming.  Note
+    // that if the generator has never sent us a message then these
+    // automatically pass!
     bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const;
 
     // these return false if a reading is not available.  They do not
@@ -52,41 +59,45 @@ public:
     bool voltage(float &voltage) const;
     bool current(float &current) const;
 
+    // healthy returns true if the generator is not present, or it is
+    // present, providing telemetry and not indicating an errors.
     bool healthy() const;
 
 private:
 
     static AP_Generator_RichenPower *_singleton;
 
+    // read - read serial port, return true if a new reading has been found
     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;
+    AP_HAL::UARTDriver *uart;
 
+    // methods and state to record pilot desired runstate and actual runstate:
     enum class RunState {
         STOP = 17,
         IDLE = 18,
         RUN = 19,
     };
-    RunState runstate = RunState::STOP;
-    void set_runstate(RunState newstate) {
+    RunState pilot_desired_runstate = RunState::STOP;
+    RunState commanded_runstate = RunState::STOP;  // output is based on this
+    void set_pilot_desired_runstate(RunState newstate) {
         // gcs().send_text(MAV_SEVERITY_INFO, "RichenPower: Moving to state (%u) from (%u)\n", (unsigned)newstate, (unsigned)runstate);
-        runstate = newstate;
+        pilot_desired_runstate = newstate;
     }
     void update_runstate();
 
+    // boolean so we can emit the RichenPower protocol version once
     bool protocol_information_anounced;
 
+    // reported mode from the generator:
     enum class Mode {
         IDLE = 0,
         RUN = 1,
         CHARGE = 2,
         BALANCE = 3,
+        OFF = 4,
     };
 
+    // un-packed data from the generator:
     struct Reading {
         uint32_t    runtime; //seconds
         uint32_t    seconds_until_maintenance;
@@ -97,7 +108,9 @@ private:
         Mode        mode;
     };
 
+    // method and state to write and entry to the onboard log:
     void Log_Write();
+    uint32_t last_logged_reading_ms;
 
     struct Reading last_reading;
     uint32_t last_reading_ms;
@@ -108,6 +121,7 @@ private:
     const uint8_t FOOTER_MAGIC1 = 0x55;
     const uint8_t FOOTER_MAGIC2 = 0xAA;
 
+    // reported errors from the generator:
     enum class Errors { // bitmask
         MaintenanceRequired = 0,
         StartDisabled = 1,
@@ -127,14 +141,16 @@ private:
     static_assert(ARRAY_SIZE(error_strings) == (uint8_t)Errors::LAST,
                   "have error string for each error");
 
+    // RichenPower data packet format:
     struct PACKED RichenPacket {
         uint8_t headermagic1;
         uint8_t headermagic2;
         uint16_t version;
-        uint8_t runtime_seconds;
         uint8_t runtime_minutes;
+        uint8_t runtime_seconds;
         uint16_t runtime_hours;
-        uint32_t seconds_until_maintenance;
+        uint16_t seconds_until_maintenance_high;
+        uint16_t seconds_until_maintenance_low;
         uint16_t errors;
         uint16_t rpm;
         uint16_t throttle;
@@ -142,8 +158,8 @@ private:
         uint16_t output_voltage;
         uint16_t output_current;
         uint16_t dynamo_current;
-        uint8_t mode;
         uint8_t unknown1;
+        uint8_t mode;
         uint8_t unknown6[38]; // "data"?!
         uint16_t checksum;
         uint8_t footermagic1;
@@ -157,23 +173,54 @@ private:
     };
     RichenUnion u;
 
+    // number of bytes currently in the buffer
     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.  The generator itself does
+    // not supply temperature via telemetry, so we fake one based on
+    // RPM.
+    uint32_t last_heat_update_ms;
+    float heat;
+    void update_heat();
+
+    // returns true if the generator should be allowed to move into
+    // the "run" (high-RPM) state:
+    bool generator_ok_to_run() const;
+    // returns an amount of synthetic heat required for the generator
+    // to move into the "run" state:
+    static constexpr float heat_required_for_run();
+
+    // approximate run and idle speeds for the generator:
     static const uint16_t RUN_RPM = 15000;
-    static const uint16_t IDLE_RPM = 4800;
-
-    // logging state
-    uint32_t last_logged_reading_ms;
+    static const uint16_t IDLE_RPM = 6000;
+
+    static constexpr float heat_environment_loss_factor = 0.005f;
+    // powf is not constexpr, so we create a const for it:
+    // powf(1.0f-heat_environment_loss_factor, 30)
+    static constexpr float heat_environment_loss_30s = 0.860384;
+    static constexpr float heat_environment_loss_60s = 0.740261;
+
+    // boolean so we can announce we've stopped the generator due to a
+    // crash just once:
+    bool vehicle_was_crashed;
+
+    // data and methods to handle time-in-idle-state:
+    uint32_t idle_state_start_ms;
+
+    uint32_t time_in_idle_state_ms() const {
+        if (idle_state_start_ms == 0) {
+            return 0;
+        }
+        return AP_HAL::millis() - idle_state_start_ms;
+    }
 };
 
 namespace AP {