Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: add support for CANFD trx

Browse Source
This commit is contained in:
Siddharth Purohit 2021-05-17 22:56:34 +05:30 committed by Andrew Tridgell
parent 72090d10da
commit 8a8de73d89
4 changed files with 80 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/AP_Periph/AP_Periph.h
View File

@ -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);

5
Tools/AP_Periph/Parameters.cpp
View File

@ -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
};

6
Tools/AP_Periph/Parameters.h
View File

@ -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() {}
};

102
Tools/AP_Periph/can.cpp
View File

@ -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,