Skip to content

Commit

Permalink
Fixed some code style formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
katzfey committed Jul 29, 2024
1 parent cc533e7 commit bcaf1c7
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 26 deletions.
2 changes: 1 addition & 1 deletion libraries/AP_HAL_QURT/UARTDriver.h
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver

private:
static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p);
uint8_t inst;
uint8_t inst;
uint32_t seq;
};

Expand Down
51 changes: 26 additions & 25 deletions libraries/AP_HAL_QURT/ap_host/src/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,29 +88,29 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes)
printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes);
return;
}
if (msg->inst < MAX_MAVLINK_INSTANCES) {
if (msg->seq != expected_seq[msg->inst]) {
printf("Invalid seq %u %u\n", msg->seq, expected_seq[msg->inst]);
}
} else {
printf("Invalid instance %u\n", msg->inst);
}
if (msg->inst < MAX_MAVLINK_INSTANCES) {
if (msg->seq != expected_seq[msg->inst]) {
printf("Invalid seq %u %u\n", msg->seq, expected_seq[msg->inst]);
}
} else {
printf("Invalid instance %u\n", msg->inst);
}
expected_seq[msg->inst] = msg->seq + 1;

switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: {
if (_running) {
if (msg->inst == 0) {
const auto bytes_sent = sendto(gcs_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&gcs_addr, sizeof(gcs_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to GCS failed\n");
}
} else if (msg->inst == 1) {
const auto bytes_sent = sendto(obd_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&obd_addr, sizeof(obd_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to onboard failed\n");
}
}
if (msg->inst == 0) {
const auto bytes_sent = sendto(gcs_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&gcs_addr, sizeof(gcs_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to GCS failed\n");
}
} else if (msg->inst == 1) {
const auto bytes_sent = sendto(obd_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&obd_addr, sizeof(obd_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to onboard failed\n");
}
}
}
break;
}
Expand Down Expand Up @@ -149,7 +149,8 @@ static void setup_directores(void)
}
}

void *obd_recv_thread(void *) {
void *obd_recv_thread(void *)
{

uint32_t next_seq = 0;

Expand Down Expand Up @@ -184,7 +185,7 @@ void *obd_recv_thread(void *) {
}
}

return NULL;
return NULL;
}

int main()
Expand Down Expand Up @@ -285,11 +286,11 @@ int main()
printf("Enter ctrl-c to exit\n");
_running = true;

pthread_t obd_recv_thread_id;
pthread_attr_t obd_recv_thread_attr;
pthread_attr_init(&obd_recv_thread_attr);
pthread_create(&obd_recv_thread_id, &obd_recv_thread_attr, obd_recv_thread, NULL);
pthread_t obd_recv_thread_id;
pthread_attr_t obd_recv_thread_attr;
pthread_attr_init(&obd_recv_thread_attr);
pthread_create(&obd_recv_thread_id, &obd_recv_thread_attr, obd_recv_thread, NULL);

uint32_t next_seq = 0;

printf("Waiting for GCS receive\n");
Expand Down

0 comments on commit bcaf1c7

Please sign in to comment.