Skip to content

Commit

Permalink
print received ODID messages both for MAVLink and DroneCAN connectors
Browse files Browse the repository at this point in the history
  • Loading branch information
BluemarkInnovations committed Feb 3, 2023
1 parent c41096a commit 012d109
Show file tree
Hide file tree
Showing 3 changed files with 5 additions and 5 deletions.
2 changes: 1 addition & 1 deletion RemoteIDModule/DroneCAN.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ void DroneCAN::arm_status_send(void)
uint8_t buffer[DRONECAN_REMOTEID_ARMSTATUS_MAX_SIZE];
dronecan_remoteid_ArmStatus arm_status {};

const uint8_t status = parse_fail==nullptr?MAV_ODID_ARM_STATUS_GOOD_TO_ARM:MAV_ODID_ARM_STATUS_PRE_ARM_FAIL_GENERIC;
const uint8_t status = parse_fail==nullptr?MAV_ODID_GOOD_TO_ARM:MAV_ODID_PRE_ARM_FAIL_GENERIC;
const char *reason = parse_fail==nullptr?"":parse_fail;

arm_status.status = status;
Expand Down
4 changes: 2 additions & 2 deletions RemoteIDModule/mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
break;
}
case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION: {
mavlink_msg_open_drone_id_location_decode(&msg, &location);
mavlink_msg_open_drone_id_location_decode(&msg, &location);
Serial.printf("MAVLink: got Location\n");
if (last_location_timestamp != location.timestamp) {
//only update the timestamp if we receive information with a different timestamp
Expand Down Expand Up @@ -280,7 +280,7 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &

void MAVLinkSerial::arm_status_send(void)
{
const uint8_t status = parse_fail==nullptr?MAV_ODID_ARM_STATUS_GOOD_TO_ARM:MAV_ODID_ARM_STATUS_PRE_ARM_FAIL_GENERIC;
const uint8_t status = parse_fail==nullptr?MAV_ODID_GOOD_TO_ARM:MAV_ODID_PRE_ARM_FAIL_GENERIC;
const char *reason = parse_fail==nullptr?"":parse_fail;
mavlink_msg_open_drone_id_arm_status_send(
chan,
Expand Down
4 changes: 2 additions & 2 deletions RemoteIDModule/transport.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ uint8_t Transport::arm_status_check(const char *&reason)
const uint32_t max_age_other_ms = 22000;
const uint32_t now_ms = millis();

uint8_t status = MAV_ODID_ARM_STATUS_PRE_ARM_FAIL_GENERIC;
uint8_t status = MAV_ODID_PRE_ARM_FAIL_GENERIC;

if (last_location_ms == 0 || now_ms - last_location_ms > max_age_location_ms) {
reason = "missing location message";
Expand All @@ -56,7 +56,7 @@ uint8_t Transport::arm_status_check(const char *&reason)
} else if (system.operator_latitude == 0 && system.operator_longitude == 0) {
reason = "Bad operator location";
} else if (reason == nullptr) {
status = MAV_ODID_ARM_STATUS_GOOD_TO_ARM;
status = MAV_ODID_GOOD_TO_ARM;
}

return status;
Expand Down

0 comments on commit 012d109

Please sign in to comment.