Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  *
      15             :  * Author: Oliver Walters / Currawong Engineering Pty Ltd
      16             :  */
      17             : 
      18             : #pragma GCC optimize("Os")
      19             : 
      20             : #include "AP_Generator_config.h"
      21             : 
      22             : #if AP_GENERATOR_CORTEX_ENABLED
      23             : 
      24             : #include "AP_Generator_Cortex.h"
      25             : #include <AP_PiccoloCAN/AP_PiccoloCAN_Device.h>
      26             : 
      27             : #include <AP_Logger/AP_Logger.h>
      28             : #include <GCS_MAVLink/GCS.h>
      29             : 
      30             : #include <AP_Param/AP_Param.h>
      31             : 
      32             : extern const AP_HAL::HAL& hal;
      33             : 
      34             : AP_Generator_Cortex* AP_Generator_Cortex::_singleton;
      35             : 
      36             : 
      37           0 : void AP_Generator_Cortex::init()
      38             : {
      39           0 :     _singleton = this;
      40             : 
      41             :     // Inform frontend which measurements are available for this generator
      42           0 :     _frontend._has_current = true;
      43           0 :     _frontend._has_consumed_energy = false;
      44           0 :     _frontend._has_fuel_remaining = false;
      45           0 : }
      46             : 
      47             : 
      48             : // Decode received CAN frame    
      49           0 : bool AP_Generator_Cortex::handle_message(AP_HAL::CANFrame &frame, AP_PiccoloCAN& can_iface)
      50             : {
      51             :     // Store a pointer to the CAN interface
      52           0 :     if (_can_iface == nullptr) {
      53           0 :         _can_iface = &can_iface;
      54             :     }
      55             : 
      56           0 :     if (decodeCortex_TelemetryStatusPacketStructure(&frame, &telemetry.status)) {
      57           0 :     } else if (decodeCortex_TelemetryGeneratorPacketStructure(&frame, &telemetry.generator)) {
      58           0 :     } else if (decodeCortex_TelemetryBatteryPacketStructure(&frame, &telemetry.battery)) {
      59           0 :     } else if (decodeCortex_TelemetryOutputRailPacketStructure(&frame, &telemetry.rails)) {
      60           0 :     } else if (decodeCortex_TelemetryControllerPacketStructure(&frame, &telemetry.controller)) {
      61             :     } else {
      62             :         // No matching packet
      63             :         return false;
      64             :     }
      65             : 
      66             :     // Extract device type and node ID from the CAN frame
      67           0 :     _can_device_id = frame.id & 0xFF;
      68           0 :     _can_device_type = (frame.id >> 8) & 0xFF;
      69             : 
      70           0 :     last_reading_ms = AP_HAL::millis();
      71             : 
      72           0 :     return true;
      73             : }
      74             : 
      75             : 
      76           0 : void AP_Generator_Cortex::update()
      77             : {
      78           0 :     if (connected != is_connected()) {
      79           0 :         connected = is_connected();
      80             : 
      81           0 :         if (connected) {
      82           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Cortex generator connected");
      83             :         } else {
      84           0 :             GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Cortex generator disconnected");
      85             :         }
      86             :     }
      87             : 
      88             :     // Update internal readings
      89           0 :     _voltage = telemetry.generator.voltage;
      90           0 :     _current = telemetry.generator.current;
      91           0 :     _rpm = (uint16_t) abs(telemetry.generator.rpm);
      92             : 
      93           0 :     update_frontend();
      94           0 : }
      95             : 
      96             : 
      97           0 : bool AP_Generator_Cortex::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
      98             : {
      99           0 :     if (!is_connected()) {
     100           0 :         hal.util->snprintf(failmsg, failmsg_len, "Generator is not connected");
     101           0 :         return false;
     102             :     }
     103             : 
     104           0 :     if (is_inhibited()) {
     105           0 :         hal.util->snprintf(failmsg, failmsg_len, "Generator is inhibited");
     106           0 :         return false;
     107             :     }
     108             : 
     109           0 :     if (!is_ready()) {
     110           0 :         hal.util->snprintf(failmsg, failmsg_len, "Generator is not ready");
     111           0 :         return false;
     112             :     }
     113             : 
     114             :     return true;
     115             : }
     116             : 
     117             : 
     118           0 : void AP_Generator_Cortex::send_generator_status(const GCS_MAVLINK &channel)
     119             : {
     120             :     uint64_t status_flags = 0;
     121             : 
     122           0 :     if (!is_connected()) {
     123             :         return;
     124             :     }
     125             : 
     126             :     // Copy status flags from MAV_GENERATOR_STATUS_FLAG enum
     127             :     const Cortex_StatusBits_t &status = telemetry.status.status;
     128             :     const Cortex_WarningBits_t &warning = telemetry.status.warning;
     129             :     const Cortex_ErrorBits_t &error = telemetry.status.error;
     130             : 
     131             :     // Check inihibit state
     132           0 :     if (status.inhibited) {
     133             :         status_flags |= MAV_GENERATOR_STATUS_FLAG_START_INHIBITED;
     134             :     }
     135             :     
     136           0 :     switch (status.mode) {
     137             :         case CORTEX_MODE_BOOTUP:
     138             :             break;
     139           0 :         case CORTEX_MODE_STANDBY:
     140           0 :             status_flags |= MAV_GENERATOR_STATUS_FLAG_IDLE;
     141           0 :             if (status.readyToRun) {
     142           0 :                 status_flags |= MAV_GENERATOR_STATUS_FLAG_READY;
     143             :             }
     144             :             break;
     145             :         case CORTEX_MODE_PREFLIGHT:
     146             :             break;
     147             :         case CORTEX_MODE_CRANKING:
     148             :             break;
     149             :         case CORTEX_MODE_RESERVED:
     150             :             break;
     151           0 :         case CORTEX_MODE_RUNNING:
     152           0 :                 if (telemetry.generator.current <= 0.05f) {
     153           0 :                     status_flags |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
     154             :                 }
     155             :             break;
     156             :     }
     157             : 
     158             :     // Status flags
     159           0 :     if (status.readyToRun) {
     160           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_READY;
     161             :     }
     162             : 
     163           0 :     if (telemetry.battery.current <= 0.1f) {
     164           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_CHARGING;
     165             :     }
     166             : 
     167           0 :     if (status.powerLimit) {
     168           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_REDUCED_POWER;
     169             :     }
     170             : 
     171             :     // Warning flags
     172           0 :     if (warning.generatorTempLimit) {
     173           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_OVERTEMP_WARNING;
     174             :     }
     175             : 
     176           0 :     if (warning.temperature) {
     177           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_ELECTRONICS_OVERTEMP_WARNING;
     178             :     }
     179             : 
     180           0 :     if (warning.enginePowerLimit) {
     181           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_MAXPOWER;
     182             :     }
     183             :     
     184             :     // Error flags
     185           0 :     if (error.generator) {
     186           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_ELECTRONICS_FAULT;
     187             :     }
     188             : 
     189           0 :     if (error.avionicsRail ||
     190           0 :         error.payloadRail ||
     191           0 :         error.servoRail ||
     192           0 :         error.batteryChargerRail) {
     193           0 :         status_flags |= MAV_GENERATOR_STATUS_FLAG_POWER_RAIL_FAULT;
     194             :     }
     195             : 
     196           0 :     mavlink_msg_generator_status_send(
     197             :         channel.get_chan(),
     198             :         status_flags,
     199           0 :         (uint16_t) abs(telemetry.generator.rpm),
     200             :         batteryCurrent(),
     201             :         loadCurrent(),
     202             :         generatorPower(),
     203             :         generatorVoltage(),
     204           0 :         rectifierTemperature(),
     205           0 :         std::numeric_limits<double>::quiet_NaN(),
     206           0 :         telemetry.generator.temperature,
     207             :         telemetry.controller.runTime,
     208             :         0  // time until maintenance (not supported)
     209             :     );
     210             : }
     211             : 
     212             : 
     213           0 : bool AP_Generator_Cortex::is_connected(void) const
     214             : {
     215           0 :     const uint32_t now = AP_HAL::millis();
     216             : 
     217           0 :     return (now - last_reading_ms) < 1000;
     218             : }
     219             : 
     220             : 
     221           0 : int16_t AP_Generator_Cortex::rectifierTemperature(void) const
     222             : {
     223             :     const Cortex_TelemetryController_t &controller = telemetry.controller;
     224             : 
     225             :     // Return the maximum of the two internal temperature values
     226           0 :     return MAX(controller.rectifierTemperature, controller.regulatorTemperature);
     227             : }
     228             : 
     229             : 
     230           0 : bool AP_Generator_Cortex::send_message(AP_HAL::CANFrame &frame)
     231             : {
     232             : 
     233           0 :     if (_can_iface == nullptr) {
     234             :         return false;
     235             :     }
     236             : 
     237           0 :     if (!is_connected()) {
     238             :         return false;
     239             :     }
     240             : 
     241             :     // Encode stored device type and node ID
     242           0 :     frame.id |= _can_device_id;
     243           0 :     frame.id |= (_can_device_type << 8) & 0xFF00;
     244             : 
     245           0 :     return _can_iface->write_frame(frame, 1000);
     246             : }
     247             : 
     248             : 
     249             : // Send the "standby" command to the Cortex generator
     250           0 : bool AP_Generator_Cortex::stop(void)
     251             : {
     252             :     // Note: If the engine is not also disabled, the generator may restart automatically
     253           0 :     if (!is_connected()) {
     254             :         return false;
     255             :     }
     256             : 
     257           0 :     AP_HAL::CANFrame txFrame {};
     258             : 
     259           0 :     encodeCortex_StandbyPacket(&txFrame);
     260           0 :     return send_message(txFrame);
     261             : }
     262             : 
     263             : 
     264             : // Send the "preflight" command to the Cortex generator
     265           0 : bool AP_Generator_Cortex::idle(void)
     266             : {
     267           0 :     if (!is_connected()) {
     268             :         return false;
     269             :     }
     270             : 
     271           0 :     AP_HAL::CANFrame txFrame {};
     272             : 
     273           0 :     encodeCortex_PreflightPacket(&txFrame);
     274             : 
     275           0 :     return send_message(txFrame);
     276             : }
     277             : 
     278             : 
     279             : // Send the "crank engine" command to the Cortex generator
     280           0 : bool AP_Generator_Cortex::run(void)
     281             : {
     282           0 :     if (!is_connected()) {
     283             :         return false;
     284             :     }
     285             : 
     286           0 :     AP_HAL::CANFrame txFrame {};
     287             : 
     288           0 :     encodeCortex_StartCrankingPacket(&txFrame);
     289             : 
     290           0 :     return send_message(txFrame);
     291             : }
     292             : 
     293             : #endif // AP_GENERATOR_CORTEX_ENABLED