diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.cpp b/libraries/AP_Generator/AP_Generator_RichenPower.cpp index 234d0464bf..e0fed44032 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.cpp +++ b/libraries/AP_Generator/AP_Generator_RichenPower.cpp @@ -21,13 +21,13 @@ #include #include #include +#include #include 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) -{ - 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; -} - +// log generator status to the onboard log void AP_Generator_RichenPower::Log_Write() { +#define MASK_LOG_ANY 0xFFFF + if (!AP::logger().should_log(MASK_LOG_ANY)) { + return; + } 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::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 ); } diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.h b/libraries/AP_Generator/AP_Generator_RichenPower.h index ef1d2f62d0..b8525deb6f 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.h +++ b/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 ¤t) 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; - static const uint16_t RUN_RPM = 15000; - static const uint16_t IDLE_RPM = 4800; + // 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(); - // logging state - uint32_t last_logged_reading_ms; + // 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 = 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 {