diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index b6ad204329..35baaa0cc2 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -19,49 +19,14 @@ #include #include #include "AP_Periph.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-align" #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#pragma GCC diagnostic pop #include #include #include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include "../AP_Bootloader/app_comms.h" @@ -212,7 +177,7 @@ static uint16_t get_random_range(uint16_t range) */ static void readUniqueID(uint8_t* out_uid) { - uint8_t len = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH; + uint8_t len = sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data); memset(out_uid, 0, len); hal.util->get_system_id_unformatted(out_uid, len); } @@ -245,14 +210,12 @@ static void handle_get_node_info(CanardInstance* ins, pkt.hardware_version.major = APJ_BOARD_ID >> 8; pkt.hardware_version.minor = APJ_BOARD_ID & 0xFF; - char text[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_NAME_MAX_LENGTH+1]; if (periph.g.serial_number > 0) { - hal.util->snprintf(text, sizeof(text), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)periph.g.serial_number); + hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)periph.g.serial_number); } else { - hal.util->snprintf(text, sizeof(text), "%s", CAN_APP_NODE_NAME); + hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s", CAN_APP_NODE_NAME); } - pkt.name.len = strlen(text); - pkt.name.data = (uint8_t *)text; + pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); @@ -279,28 +242,25 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) stm32_watchdog_pat(); uavcan_protocol_param_GetSetRequest req; - uint8_t arraybuf[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_NAME_MAX_LENGTH]; - uint8_t *arraybuf_ptr = arraybuf; - if (uavcan_protocol_param_GetSetRequest_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) { + if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) { return; } uavcan_protocol_param_GetSetResponse pkt {}; - uint8_t name[AP_MAX_NAME_SIZE+1] {}; AP_Param *vp; enum ap_var_type ptype; if (req.name.len != 0 && req.name.len > AP_MAX_NAME_SIZE) { vp = nullptr; } else if (req.name.len != 0 && req.name.len <= AP_MAX_NAME_SIZE) { - strncpy((char *)name, (char *)req.name.data, req.name.len); - vp = AP_Param::find((char *)name, &ptype); + memcpy((char *)pkt.name.data, (char *)req.name.data, req.name.len); + vp = AP_Param::find((char *)pkt.name.data, &ptype); } else { AP_Param::ParamToken token {}; vp = AP_Param::find_by_index(req.index, &ptype, &token); if (vp != nullptr) { - vp->copy_name_token(token, (char *)name, AP_MAX_NAME_SIZE+1, true); + vp->copy_name_token(token, (char *)pkt.name.data, AP_MAX_NAME_SIZE+1, true); } } if (vp != nullptr && req.name.len != 0 && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) { @@ -355,8 +315,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) default: return; } - pkt.name.len = strlen((char *)name); - pkt.name.data = name; + pkt.name.len = strnlen((char *)pkt.name.data, sizeof(pkt.name.data)); } uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; @@ -380,7 +339,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_protocol_param_ExecuteOpcodeRequest req; - if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, transfer->payload_len, &req, nullptr) < 0) { + if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { return; } if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) { @@ -507,38 +466,31 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr } // Copying the unique ID from the message - static const uint8_t UniqueIDBitOffset = 8; - uint8_t received_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; - uint8_t received_unique_id_len = 0; - for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) { - const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U); - (void) canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); - } + uavcan_protocol_dynamic_node_id_Allocation msg; + + uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg); // Obtaining the local unique ID - uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; + uint8_t my_unique_id[sizeof(msg.unique_id.data)]; readUniqueID(my_unique_id); // Matching the received UID against the local one - if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { + if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { printf("Mismatching allocation response\n"); can_ins->node_id_allocation_unique_id_offset = 0; return; // No match, return } - if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) { + if (msg.unique_id.len < sizeof(msg.unique_id.data)) { // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. - can_ins->node_id_allocation_unique_id_offset = received_unique_id_len; + can_ins->node_id_allocation_unique_id_offset = msg.unique_id.len; can_ins->send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; - printf("Matching allocation response: %d\n", received_unique_id_len); + printf("Matching allocation response: %d\n", msg.unique_id.len); } else { // Allocation complete - copying the allocated node ID from the message - uint8_t allocated_node_id = 0; - (void) canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); - - canardSetLocalNodeID(ins, allocated_node_id); - printf("IF%d Node ID allocated: %d\n", can_ins->index, allocated_node_id); + canardSetLocalNodeID(ins, msg.node_id); + printf("IF%d Node ID allocated: %d\n", can_ins->index, msg.node_id); #if defined(HAL_PERIPH_ENABLE_GPS) && (HAL_NUM_CAN_IFACES >= 2) && GPS_MOVING_BASELINE if (periph.g.gps_mb_only_can_port) { @@ -546,7 +498,7 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr periph.gps_mb_can_port = (can_ins->index+1)%HAL_NUM_CAN_IFACES; if (canardGetLocalNodeID(&instances[periph.gps_mb_can_port].canard) == CANARD_BROADCAST_NODE_ID) { // copy node id from the primary iface - canardSetLocalNodeID(&instances[periph.gps_mb_can_port].canard, allocated_node_id); + canardSetLocalNodeID(&instances[periph.gps_mb_can_port].canard, msg.node_id); #ifdef HAL_GPIO_PIN_TERMCAN1 // also terminate the line as we don't have any other device on this port palWriteLine(can_term_lines[periph.gps_mb_can_port], 1); @@ -557,23 +509,6 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr } } - -/* - fix value of a float for canard float16 format. This is only needed - for array attributes. Scalars are handled by the compiler - */ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" -static void fix_float16_array(float *f, uint8_t len) -{ - while (len--) { - *(uint16_t *)f = canardConvertNativeFloatToFloat16(*f); - f++; - } -} -#pragma GCC diagnostic pop - - #if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) static uint32_t buzzer_start_ms; static uint32_t buzzer_len_ms; @@ -583,7 +518,7 @@ static uint32_t buzzer_len_ms; static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_indication_BeepCommand req; - if (uavcan_equipment_indication_BeepCommand_decode(transfer, transfer->payload_len, &req, nullptr) < 0) { + if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) { return; } static bool initialised; @@ -626,7 +561,7 @@ static uint8_t safety_state; static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) { ardupilot_indication_SafetyState req; - if (ardupilot_indication_SafetyState_decode(transfer, transfer->payload_len, &req, nullptr) < 0) { + if (ardupilot_indication_SafetyState_decode(transfer, &req)) { return; } safety_state = req.status; @@ -642,7 +577,7 @@ static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_safety_ArmingStatus req; - if (uavcan_equipment_safety_ArmingStatus_decode(transfer, transfer->payload_len, &req, nullptr) < 0) { + if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &req)) { return; } hal.util->set_soft_armed(req.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED); @@ -655,9 +590,7 @@ static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_gnss_RTCMStream req; - uint8_t arraybuf[UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_DATA_MAX_LENGTH]; - uint8_t *arraybuf_ptr = arraybuf; - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) { + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) { return; } periph.gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len); @@ -670,9 +603,7 @@ static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) static void handle_MovingBaselineData(CanardInstance* ins, CanardRxTransfer* transfer) { ardupilot_gnss_MovingBaselineData msg; - uint8_t arraybuf[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE]; - uint8_t *arraybuf_ptr = arraybuf; - if (ardupilot_gnss_MovingBaselineData_decode(transfer, transfer->payload_len, &msg, &arraybuf_ptr) < 0) { + if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) { return; } periph.gps.inject_MBL_data(msg.data.data, msg.data.len); @@ -771,9 +702,7 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_indication_LightsCommand req; - uint8_t arraybuf[UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_MAX_SIZE]; - uint8_t *arraybuf_ptr = arraybuf; - if (uavcan_equipment_indication_LightsCommand_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) { + if (uavcan_equipment_indication_LightsCommand_decode(transfer, &req)) { return; } for (uint8_t i=0; ipayload_len, &cmd, &arraybuf_ptr) < 0) { + if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) { return; } periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); @@ -1444,11 +1371,11 @@ static bool can_do_dna(instance_t &ins) allocation_request[0] |= 1; // First part of unique ID } - uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; + uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)]; readUniqueID(my_unique_id); static const uint8_t MaxLenOfUniqueIDInRequest = 6; - uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - ins.node_id_allocation_unique_id_offset); + uint8_t uid_size = (uint8_t)(sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data) - ins.node_id_allocation_unique_id_offset); if (uid_size > MaxLenOfUniqueIDInRequest) { uid_size = MaxLenOfUniqueIDInRequest; @@ -1711,7 +1638,6 @@ void AP_Periph_FW::can_mag_update(void) for (uint8_t i=0; i<3; i++) { pkt.magnetic_field_ga[i] = field[i] * 0.001; } - fix_float16_array(pkt.magnetic_field_ga, 3); uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer); @@ -1770,10 +1696,8 @@ void AP_Periph_FW::can_battery_update(void) #if !defined(HAL_PERIPH_BATTERY_SKIP_NAME) // example model_name: "org.ardupilot.ap_periph SN 123" - char text[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MODEL_NAME_MAX_LENGTH+1] {}; - hal.util->snprintf(text, sizeof(text), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number); - pkt.model_name.len = strlen(text); - pkt.model_name.data = (uint8_t *)text; + hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number); + pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data)); #endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; @@ -1828,7 +1752,6 @@ void AP_Periph_FW::can_gps_update(void) // the canard dsdl compiler doesn't understand float16 pkt.ned_velocity[i] = vel[i]; } - fix_float16_array(pkt.ned_velocity, 3); pkt.sats_used = gps.num_sats(); switch (gps.status()) { case AP_GPS::GPS_Status::NO_GPS: @@ -1846,31 +1769,25 @@ void AP_Periph_FW::can_gps_update(void) break; } - float pos_cov[9] {}; - pkt.position_covariance.data = &pos_cov[0]; pkt.position_covariance.len = 9; float vacc; if (gps.vertical_accuracy(vacc)) { - pos_cov[8] = sq(vacc); + pkt.position_covariance.data[8] = sq(vacc); } float hacc; if (gps.horizontal_accuracy(hacc)) { - pos_cov[0] = pos_cov[4] = sq(hacc); + pkt.position_covariance.data[0] = pkt.position_covariance.data[4] = sq(hacc); } - fix_float16_array(pos_cov, 9); - float vel_cov[9] {}; - pkt.velocity_covariance.data = &pos_cov[0]; pkt.velocity_covariance.len = 9; float sacc; if (gps.speed_accuracy(sacc)) { float vc3 = sq(sacc); - vel_cov[0] = vel_cov[4] = vel_cov[8] = vc3; + pkt.velocity_covariance.data[0] = pkt.velocity_covariance.data[4] = pkt.velocity_covariance.data[8] = vc3; } - fix_float16_array(vel_cov, 9); uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer); @@ -1939,26 +1856,23 @@ void AP_Periph_FW::can_gps_update(void) break; } - float cov[6] {}; - pkt.covariance.data = &cov[0]; pkt.covariance.len = 6; float hacc; if (gps.horizontal_accuracy(hacc)) { - cov[0] = cov[1] = sq(hacc); + pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc); } float vacc; if (gps.vertical_accuracy(vacc)) { - cov[2] = sq(vacc); + pkt.covariance.data[2] = sq(vacc); } float sacc; if (gps.speed_accuracy(sacc)) { float vc3 = sq(sacc); - cov[3] = cov[4] = cov[5] = vc3; + pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; } - fix_float16_array(cov, 6); uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer); @@ -2032,9 +1946,9 @@ void AP_Periph_FW::send_moving_baseline_msg() // send the packet from Moving Base to be used RelPosHeading calc by GPS module ardupilot_gnss_MovingBaselineData mbldata {}; // get the data from the moving base - static_assert(ARDUPILOT_GNSS_MOVINGBASELINEDATA_DATA_MAX_LENGTH == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); + static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); mbldata.data.len = len; - mbldata.data.data = (uint8_t*)data; + 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); @@ -2303,7 +2217,6 @@ void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) pkt.velocity[0] = cosf(pkt.heading) * msg.hor_velocity * 1e-2; pkt.velocity[1] = sinf(pkt.heading) * msg.hor_velocity * 1e-2; pkt.velocity[2] = -msg.ver_velocity * 1e-2; - fix_float16_array(pkt.velocity, 3); pkt.squawk = msg.squawk; memcpy(pkt.callsign, msg.callsign, MIN(sizeof(msg.callsign),sizeof(pkt.callsign))); @@ -2350,13 +2263,11 @@ void can_printf(const char *fmt, ...) { uavcan_protocol_debug_LogMessage pkt {}; uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {}; - char tbuf[100]; va_list ap; va_start(ap, fmt); - uint32_t n = vsnprintf(tbuf, sizeof(tbuf), fmt, ap); + uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap); va_end(ap); - pkt.text.len = MIN(n, sizeof(tbuf)); - pkt.text.data = (uint8_t *)&tbuf[0]; + pkt.text.len = MIN(n, sizeof(pkt.text.data)); uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 5bef001980..a3e5f8d424 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -57,7 +57,10 @@ def build(bld): ) # build external libcanard library - bld.stlib(source='../../modules/libcanard/canard.c', + bld.stlib(source=['../../modules/libcanard/canard.c'] + + bld.bldnode.ant_glob('modules/libcanard/dsdlc_generated/src/**.c'), + includes=[bld.env.SRCROOT + '/modules/libcanard', + bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated/include'], target='libcanard') bld.ap_program( @@ -65,11 +68,5 @@ def build(bld): use=['AP_Periph_libs', 'libcanard'], program_groups=['bin','AP_Periph'], includes=[bld.env.SRCROOT + '/modules/libcanard', - bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated'] - ) - - bld( - # build libcanard headers - rule="python3 ${SRCROOT}/modules/libcanard/dsdl_compiler/libcanard_dsdlc --header_only --outdir ${BUILDROOT}/modules/libcanard/dsdlc_generated ${SRCROOT}/modules/uavcan/dsdl/uavcan ${SRCROOT}/libraries/AP_UAVCAN/dsdl/ardupilot ${SRCROOT}/libraries/AP_UAVCAN/dsdl/com", - group='dynamic_sources', + bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated/include'] )