Line data    Source code

       1             : // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
       2             : #pragma once
       3             : 
       4             : #include "AP_Generator_config.h"
       5             : 
       6             : #if AP_GENERATOR_RICHENPOWER_ENABLED
       7             : 
       8             : #include "AP_Generator_Backend.h"
       9             : 
      10             : #include <AP_Logger/AP_Logger_config.h>
      11             : #include <AP_Common/AP_Common.h>
      12             : #include <stdint.h>
      13             : #include <stdio.h>
      14             : 
      15             : 
      16             : /*
      17             :  *  Example setup:
      18             :  *  param set SERIAL2_PROTOCOL 30  # Generator protocol
      19             :  *  param set SERIAL2_BAUD 9600
      20             :  *  param set RC9_OPTION 85        # pilot directive for generator
      21             :  *  param set SERVO8_FUNCTION 42   # autopilot directive to generator
      22             :  */
      23             : 
      24             : class AP_Generator_RichenPower : public AP_Generator_Backend
      25             : {
      26             : 
      27             : public:
      28             :     // constructor
      29             :     using AP_Generator_Backend::AP_Generator_Backend;
      30             : 
      31             :     // init should be called at vehicle startup to get the generator library ready
      32             :     __INITFUNC__ void init(void) override;
      33             :     // update should be called regularly to update the generator state
      34             :     void update(void) override;
      35             : 
      36             :     // methods to control the generator state:
      37             :     bool stop(void) override;
      38             :     bool idle(void) override;
      39             :     bool run(void) override;
      40             : 
      41             :     // method to send a GENERATOR_STATUS mavlink message
      42             :     void send_generator_status(const GCS_MAVLINK &channel) override;
      43             : 
      44             :     // prearm checks to ensure generator is good for arming.  Note
      45             :     // that if the generator has never sent us a message then these
      46             :     // automatically pass!
      47             :     bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const override;
      48             : 
      49             :     // Update front end with voltage, current, and rpm values
      50             :     void update_frontend_readings(void);
      51             : 
      52             :     // healthy returns true if the generator is not present, or it is
      53             :     // present, providing telemetry and not indicating an errors.
      54             :     bool healthy() const override;
      55             : 
      56             : private:
      57             : 
      58             :     // read - read serial port, return true if a new reading has been found
      59             :     bool get_reading();
      60             :     AP_HAL::UARTDriver *uart;
      61             : 
      62             :     // methods and state to record pilot desired runstate and actual runstate:
      63             :     enum class RunState {
      64             :         STOP = 17,
      65             :         IDLE = 18,
      66             :         RUN = 19,
      67             :     };
      68             :     RunState pilot_desired_runstate = RunState::STOP;
      69             :     RunState commanded_runstate = RunState::STOP;  // output is based on this
      70           9 :     void set_pilot_desired_runstate(RunState newstate) {
      71             :         // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RichenPower: Moving to state (%u) from (%u)", (unsigned)newstate, (unsigned)runstate);
      72           9 :         pilot_desired_runstate = newstate;
      73           9 :     }
      74             :     void update_runstate();
      75             : 
      76             :     // boolean so we can emit the RichenPower protocol version once
      77             :     bool protocol_information_anounced;
      78             : 
      79             :     // reported mode from the generator:
      80             :     enum class Mode {
      81             :         IDLE = 0,
      82             :         RUN = 1,
      83             :         CHARGE = 2,
      84             :         BALANCE = 3,
      85             :         OFF = 4,
      86             :     };
      87             : 
      88             :     // un-packed data from the generator:
      89             :     struct Reading {
      90             :         uint32_t    runtime; //seconds
      91             :         uint32_t    seconds_until_maintenance;
      92             :         uint16_t    errors;
      93             :         uint16_t    rpm;
      94             :         float       output_voltage;
      95             :         float       output_current;
      96             :         Mode        mode;
      97             :     };
      98             : 
      99             : #if HAL_LOGGING_ENABLED
     100             :     // method and state to write and entry to the onboard log:
     101             :     void Log_Write();
     102             :     uint32_t last_logged_reading_ms;
     103             : #endif
     104             : 
     105             :     struct Reading last_reading;
     106             :     uint32_t last_reading_ms;
     107             : 
     108             :     static constexpr uint8_t HEADER_MAGIC1 = 0xAA;
     109             :     static constexpr uint8_t HEADER_MAGIC2 = 0x55;
     110             : 
     111             :     static constexpr uint8_t FOOTER_MAGIC1 = 0x55;
     112             :     static constexpr uint8_t FOOTER_MAGIC2 = 0xAA;
     113             : 
     114             :     // reported errors from the generator:
     115             :     enum class Errors { // bitmask
     116             :         MaintenanceRequired = 0,
     117             :         StartDisabled = 1,
     118             :         Overload = 2,
     119             :         LowVoltageOutput = 3,
     120             :         LowBatteryVoltage = 4,
     121             :         LAST
     122             :     };
     123             : 
     124             :     const char *error_strings[5] = {
     125             :         "MaintenanceRequired",
     126             :         "StartDisabled",
     127             :         "Overload",
     128             :         "LowVoltageOutput",
     129             :         "LowBatteryVoltage",
     130             :     };
     131             :     static_assert(ARRAY_SIZE(error_strings) == (uint8_t)Errors::LAST,
     132             :                   "have error string for each error");
     133             : 
     134             :     // RichenPower data packet format:
     135             :     struct PACKED RichenPacket {
     136             :         uint8_t headermagic1;
     137             :         uint8_t headermagic2;
     138             :         uint16_t version;
     139             :         uint8_t runtime_minutes;
     140             :         uint8_t runtime_seconds;
     141             :         uint16_t runtime_hours;
     142             :         uint16_t seconds_until_maintenance_high;
     143             :         uint16_t seconds_until_maintenance_low;
     144             :         uint16_t errors;
     145             :         uint16_t rpm;
     146             :         uint16_t throttle;
     147             :         uint16_t idle_throttle;
     148             :         uint16_t output_voltage;
     149             :         uint16_t output_current;
     150             :         uint16_t dynamo_current;
     151             :         uint8_t unknown1;
     152             :         uint8_t mode;
     153             :         uint8_t unknown6[38]; // "data"?!
     154             :         uint16_t checksum;
     155             :         uint8_t footermagic1;
     156             :         uint8_t footermagic2;
     157             :     };
     158             : 
     159             :     union RichenUnion {
     160             :         uint8_t parse_buffer[70];
     161             :         struct RichenPacket packet;
     162             :     };
     163             :     RichenUnion u;
     164             : 
     165             :     // number of bytes currently in the buffer
     166             :     uint8_t body_length;
     167             : 
     168             :     // move the expected header bytes into &buffer[0], adjusting
     169             :     // body_length as appropriate.
     170             :     void move_header_in_buffer(uint8_t initial_offset);
     171             : 
     172             :     // a simple heat model to avoid the motor moving to run too fast
     173             :     // or being stopped before cooldown.  The generator itself does
     174             :     // not supply temperature via telemetry, so we fake one based on
     175             :     // RPM.
     176             :     uint32_t last_heat_update_ms;
     177             :     float heat;
     178             :     void update_heat();
     179             : 
     180             :     // returns true if the generator should be allowed to move into
     181             :     // the "run" (high-RPM) state:
     182             :     bool generator_ok_to_run() const;
     183             :     // returns an amount of synthetic heat required for the generator
     184             :     // to move into the "run" state:
     185             :     static constexpr float heat_required_for_run();
     186             : 
     187             :     // approximate run and idle speeds for the generator:
     188             :     static const uint16_t RUN_RPM = 15000;
     189             :     static const uint16_t IDLE_RPM = 6000;
     190             : 
     191             :     static constexpr float heat_environment_loss_factor = 0.005f;
     192             :     // powf is not constexpr, so we create a const for it:
     193             :     // powf(1.0f-heat_environment_loss_factor, 30)
     194             :     static constexpr float heat_environment_loss_30s = 0.860384;
     195             :     static constexpr float heat_environment_loss_60s = 0.740261;
     196             : 
     197             :     // boolean so we can announce we've stopped the generator due to a
     198             :     // crash just once:
     199             :     bool vehicle_was_crashed;
     200             : 
     201             :     // data and methods to handle time-in-idle-state:
     202             :     uint32_t idle_state_start_ms;
     203             : 
     204           9 :     uint32_t time_in_idle_state_ms() const {
     205           9 :         if (idle_state_start_ms == 0) {
     206             :             return 0;
     207             :         }
     208           9 :         return AP_HAL::millis() - idle_state_start_ms;
     209             :     }
     210             : 
     211             :     // check if the generator requires maintenance and send a message if it does:
     212             :     void check_maintenance_required();
     213             :     // if we are emitting warnings about the generator requiring
     214             :     // maintenamce, this is the last time we sent the warning:
     215             :     uint32_t last_maintenance_warning_ms;
     216             : };
     217             : #endif  // AP_GENERATOR_RICHENPOWER_ENABLED