diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index bbaec88f3e..428f589b89 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -403,82 +403,6 @@ void AP_PiccoloCAN::update() #endif // HAL_LOGGING_ENABLED } -#if HAL_GCS_ENABLED -// send ESC telemetry messages over MAVLink -void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) -{ - // Arrays to store ESC telemetry data - uint8_t temperature[4] {}; - uint16_t voltage[4] {}; - uint16_t rpm[4] {}; - uint16_t count[4] {}; - uint16_t current[4] {}; - uint16_t totalcurrent[4] {}; - - bool dataAvailable = false; - - uint8_t idx = 0; - - WITH_SEMAPHORE(_telem_sem); - - for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) { - - // Calculate index within storage array - idx = (ii % 4); - - AP_PiccoloCAN_ESC &esc = _escs[idx]; - - // Has the ESC been heard from recently? - if (is_esc_present(ii)) { - dataAvailable = true; - - // Provide the maximum ESC temperature in the telemetry stream - temperature[idx] = esc.temperature(); // Convert to C - voltage[idx] = esc.voltage() * 10; // Convert to cV - current[idx] = esc.current() * 10; // Convert to cA - totalcurrent[idx] = 0; - rpm[idx] = esc.rpm(); - count[idx] = 0; - } else { - temperature[idx] = 0; - voltage[idx] = 0; - current[idx] = 0; - totalcurrent[idx] = 0; - rpm[idx] = 0; - count[idx] = 0; - } - - // Send ESC telemetry in groups of 4 - if ((ii % 4) == 3) { - - if (dataAvailable) { - if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) { - continue; - } - - switch (ii) { - case 3: - mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 7: - mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 11: - mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 15: - mavlink_msg_esc_telemetry_13_to_16_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - default: - break; - } - } - - dataAvailable = false; - } - } -} -#endif // send servo messages over CAN void AP_PiccoloCAN::send_servo_messages(void) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 770f05dcf5..3615f3db4f 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -56,9 +56,6 @@ public: // called from SRV_Channels void update(); - // send ESC telemetry messages over MAVLink - void send_esc_telemetry_mavlink(uint8_t mav_chan); - // return true if a particular servo is 'active' on the Piccolo interface bool is_servo_channel_active(uint8_t chan); @@ -125,14 +122,14 @@ private: } _ecu_info; // Piccolo CAN parameters - AP_Int32 _esc_bm; //! ESC selection bitmask - AP_Int16 _esc_hz; //! ESC update rate (Hz) + AP_Int32 _esc_bm; //!< ESC selection bitmask + AP_Int16 _esc_hz; //!< ESC update rate (Hz) - AP_Int32 _srv_bm; //! Servo selection bitmask - AP_Int16 _srv_hz; //! Servo update rate (Hz) + AP_Int32 _srv_bm; //!< Servo selection bitmask + AP_Int16 _srv_hz; //!< Servo update rate (Hz) - AP_Int16 _ecu_id; //! ECU Node ID - AP_Int16 _ecu_hz; //! ECU update rate (Hz) + AP_Int16 _ecu_id; //!< ECU Node ID + AP_Int16 _ecu_hz; //!< ECU update rate (Hz) HAL_Semaphore _telem_sem; }; diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h index e0b3cd9f16..fff6e17539 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h @@ -27,18 +27,18 @@ #if HAL_PICCOLO_CAN_ENABLE -// Piccolo message groups form part of the CAN ID of each frame +//! Piccolo message groups form part of the CAN ID of each frame enum class PiccoloCAN_MessageGroup : uint8_t { - SIMULATOR = 0x00, // Simulator messages - SENSOR = 0x04, // External sensors - ACTUATOR = 0x07, // Actuators (e.g. ESC / servo) - ECU_OUT = 0x08, // Messages *from* an ECU - ECU_IN = 0x09, // Message *to* an ECU + SIMULATOR = 0x00, //!< Simulator messages + SENSOR = 0x04, //!< External sensors + ACTUATOR = 0x07, //!< Actuators (e.g. ESC / servo) + ECU_OUT = 0x08, //!< Messages *from* an ECU + ECU_IN = 0x09, //!< Message *to* an ECU - SYSTEM = 0x19, // System messages (e.g. bootloader) + SYSTEM = 0x19, //!< System messages (e.g. bootloader) }; -// Piccolo actuator types differentiate between actuator frames +//! Piccolo actuator types differentiate between actuator frames enum class PiccoloCAN_ActuatorType : uint8_t { SERVO = 0x00, ESC = 0x20, diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp index 8f45dda700..24c4ed760c 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp @@ -20,7 +20,7 @@ #if HAL_PICCOLO_CAN_ENABLE /* - * Decode a recevied CAN frame. + * Decode a received CAN frame. * It is assumed at this point that the received frame is intended for *this* ESC */ bool AP_PiccoloCAN_ESC::handle_can_frame(AP_HAL::CANFrame &frame)