From 8a8de73d898850d47011f385bb658ad78a636ca1 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 17 May 2021 22:56:34 +0530 Subject: [PATCH] AP_Periph: add support for CANFD trx --- Tools/AP_Periph/AP_Periph.h | 1 + Tools/AP_Periph/Parameters.cpp | 5 ++ Tools/AP_Periph/Parameters.h | 6 ++ Tools/AP_Periph/can.cpp | 102 ++++++++++++++++++++++----------- 4 files changed, 80 insertions(+), 34 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 7454b5ef74..d1a30466cb 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -85,6 +85,7 @@ public: void load_parameters(); void prepare_reboot(); + bool canfdout() const { return (g.can_fdmode == 1); } #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT void check_for_serial_reboot_cmd(const int8_t serial_index); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index a023723043..3256980434 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -374,6 +374,11 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp GOBJECT(scripting, "SCR_", AP_Scripting), #endif + +#if HAL_CANFD_SUPPORTED + // can node FD Out mode + GSCALAR(can_fdmode, "CAN_FDMODE", 0), +#endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 2320297fd1..2bc7c46d93 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -51,6 +51,7 @@ public: k_param_gps_mb_only_can_port, k_param_scripting, k_param_esc_telem_port, + k_param_can_fdmode, }; AP_Int16 format_version; @@ -123,6 +124,11 @@ public: AP_Int16 sysid_this_mav; #endif +#if HAL_CANFD_SUPPORTED + AP_Int8 can_fdmode; +#else + static constexpr uint8_t can_fdmode = 0; +#endif Parameters() {} }; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index bdd7c5a87d..9d8a7b4949 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -219,7 +219,7 @@ static void handle_get_node_info(CanardInstance* ins, } pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !periph.canfdout()); const int16_t resp_res = canardRequestOrRespond(ins, transfer->source_node_id, @@ -229,7 +229,11 @@ static void handle_get_node_info(CanardInstance* ins, transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + , periph.canfdout() +#endif +); if (resp_res <= 0) { printf("Could not respond to GetNodeInfo: %d\n", resp_res); } @@ -321,7 +325,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) } uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !periph.canfdout()); canardRequestOrRespond(ins, transfer->source_node_id, @@ -331,7 +335,11 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif +); } @@ -374,7 +382,7 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr pkt.ok = true; uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !periph.canfdout()); canardRequestOrRespond(ins, transfer->source_node_id, @@ -384,7 +392,11 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif +); } static void canard_broadcast(uint64_t data_type_signature, @@ -423,7 +435,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* uavcan_protocol_file_BeginFirmwareUpdateResponse reply {}; reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; - uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !periph.canfdout()); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, @@ -432,7 +444,11 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif +); uint8_t count = 50; while (count--) { processTx(); @@ -858,7 +874,7 @@ static void can_safety_button_update(void) pkt.press_time = counter; uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {}; - uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer); + uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE, ARDUPILOT_INDICATION_BUTTON_ID, @@ -1138,7 +1154,11 @@ static void canard_broadcast(uint64_t data_type_signature, tid_ptr, priority, payload, - payload_len); + payload_len +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif + ); #if DEBUG_PKTS if (res < 0) { printf("Tx error %d, IF%d %lx\n", res, ins.index); @@ -1161,9 +1181,12 @@ static void processTx(void) #endif for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&ins.canard)) != NULL;) { AP_HAL::CANFrame txmsg {}; - txmsg.dlc = txf->data_len; - memcpy(txmsg.data, txf->data, 8); + txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len); + memcpy(txmsg.data, txf->data, txf->data_len); txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); +#if HAL_CANFD_SUPPORTED + txmsg.canfd = txf->canfd; +#endif // push message with 1s timeout const uint64_t deadline = AP_HAL::native_micros64() + 1000000; if (ins.iface->send(txmsg, deadline, 0) > 0) { @@ -1208,8 +1231,11 @@ static void processRx(void) uint64_t timestamp; AP_HAL::CANIface::CanIOFlags flags; ins.iface->receive(rxmsg, timestamp, flags); - memcpy(rx_frame.data, rxmsg.data, 8); - rx_frame.data_len = rxmsg.dlc; + rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); + memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); +#if HAL_CANFD_SUPPORTED + rx_frame.canfd = rxmsg.canfd; +#endif rx_frame.id = rxmsg.id; #if DEBUG_PKTS const int16_t res = @@ -1245,7 +1271,7 @@ static void node_status_send(void) node_status.vendor_specific_status_code = hal.util->available_memory(); - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, @@ -1390,7 +1416,11 @@ static bool can_do_dna(instance_t &ins) &node_id_allocation_transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &allocation_request[0], - (uint16_t) (uid_size + 1)); + (uint16_t) (uid_size + 1) +#if HAL_CANFD_SUPPORTED + ,false +#endif + ); if (bcast_res < 0) { printf("Could not broadcast ID allocation req; error %d, IF%d\n", bcast_res, ins.index); } @@ -1495,7 +1525,7 @@ void AP_Periph_FW::pwm_hardpoint_update() cmd.command = value; uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer); + uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1523,7 +1553,7 @@ void AP_Periph_FW::hwesc_telem_update() pkt.error_count = t.error_count; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1576,7 +1606,7 @@ void AP_Periph_FW::esc_telem_update() pkt.error_count = 0; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1695,7 +1725,7 @@ void AP_Periph_FW::can_mag_update(void) } uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, @@ -1756,7 +1786,7 @@ void AP_Periph_FW::can_battery_update(void) #endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; - const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer); + const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, @@ -1845,7 +1875,7 @@ void AP_Periph_FW::can_gps_update(void) } uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX_ID, @@ -1935,7 +1965,7 @@ void AP_Periph_FW::can_gps_update(void) } uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX2_ID, @@ -1953,7 +1983,7 @@ void AP_Periph_FW::can_gps_update(void) aux.vdop = gps.get_vdop() * 0.01; uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer); + uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1980,7 +2010,7 @@ void AP_Periph_FW::can_gps_update(void) } uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer); + const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, ARDUPILOT_GNSS_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -2010,7 +2040,7 @@ void AP_Periph_FW::send_moving_baseline_msg() mbldata.data.len = len; memcpy(mbldata.data.data, data, len); uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer); + const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !periph.canfdout()); #if HAL_NUM_CAN_IFACES >= 2 if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { @@ -2021,7 +2051,11 @@ void AP_Periph_FW::send_moving_baseline_msg() tid_ptr, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,canfdout() +#endif + ); } else #endif { @@ -2056,7 +2090,7 @@ void AP_Periph_FW::send_relposheading_msg() { relpos.reported_heading_acc_deg = reported_heading_acc; relpos.reported_heading_acc_available = true; uint8_t buffer[ARDUPILOT_GNSS_RELPOSHEADING_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer); + const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE, ARDUPILOT_GNSS_RELPOSHEADING_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -2093,7 +2127,7 @@ void AP_Periph_FW::can_baro_update(void) pkt.static_pressure_variance = 0; // should we make this a parameter? uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID, @@ -2108,7 +2142,7 @@ void AP_Periph_FW::can_baro_update(void) pkt.static_temperature_variance = 0; // should we make this a parameter? uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID, @@ -2169,7 +2203,7 @@ void AP_Periph_FW::can_airspeed_update(void) pkt.pitot_temperature = nanf(""); uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, @@ -2247,7 +2281,7 @@ void AP_Periph_FW::can_rangefinder_update(void) pkt.range = dist_cm * 0.01; uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, @@ -2308,7 +2342,7 @@ void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) pkt.baro_valid = (msg.flags & 0x0100) != 0; uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {}; - uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer); + uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE, ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID, @@ -2329,7 +2363,7 @@ void can_printf(const char *fmt, ...) va_end(ap); pkt.text.len = MIN(n, sizeof(pkt.text.data)); - uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); + uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,