From 68f463b29d18888973eba9f30ca19d13d7399373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 12 Oct 2018 10:35:04 +1100 Subject: [PATCH] AP_Radio: use WITH_SEMAPHORE() and removed usage of hal.util->new_semaphore() --- libraries/AP_Radio/AP_Radio_cc2500.cpp | 54 ++++++++++++------------- libraries/AP_Radio/AP_Radio_cc2500.h | 2 +- libraries/AP_Radio/AP_Radio_cypress.cpp | 18 ++++----- libraries/AP_Radio/AP_Radio_cypress.h | 2 +- 4 files changed, 36 insertions(+), 40 deletions(-) diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index ef8d6c243a..f61a78bca6 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -68,8 +68,6 @@ bool AP_Radio_cc2500::init(void) irq_handler_thd, /* Thread function. */ NULL); /* Thread parameter. */ #endif - sem = hal.util->new_semaphore(); - return reset(); } @@ -340,26 +338,26 @@ void AP_Radio_cc2500::handle_data_packet(mavlink_channel_t chan, const mavlink_d uint32_t ofs=0; memcpy(&ofs, &m.data[0], 4); Debug(4, "got data96 of len %u from chan %u at offset %u\n", m.len, chan, unsigned(ofs)); - if (sem->take_nonblocking()) { - fwupload.chan = chan; - fwupload.need_ack = false; - fwupload.offset = ofs; - fwupload.length = MIN(m.len-4, 92); - fwupload.acked = 0; - fwupload.sequence++; - if (m.type == 43) { - // sending a tune to play - for development testing - fwupload.fw_type = TELEM_PLAY; - fwupload.length = MIN(m.len, 90); - fwupload.offset = 0; - memcpy(&fwupload.pending_data[0], &m.data[0], fwupload.length); - } else { - // sending a chunk of firmware OTA upload - fwupload.fw_type = TELEM_FW; - memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); - } - sem->give(); - } + + WITH_SEMAPHORE(sem); + + fwupload.chan = chan; + fwupload.need_ack = false; + fwupload.offset = ofs; + fwupload.length = MIN(m.len-4, 92); + fwupload.acked = 0; + fwupload.sequence++; + if (m.type == 43) { + // sending a tune to play - for development testing + fwupload.fw_type = TELEM_PLAY; + fwupload.length = MIN(m.len, 90); + fwupload.offset = 0; + memcpy(&fwupload.pending_data[0], &m.data[0], fwupload.length); + } else { + // sending a chunk of firmware OTA upload + fwupload.fw_type = TELEM_FW; + memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); + } } /* @@ -462,14 +460,14 @@ bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet) // got an fw upload ack Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", data, fwupload.sequence, unsigned(fwupload.acked), unsigned(fwupload.length), fwupload.len); - if (fwupload.sequence == data && sem->take_nonblocking()) { + if (fwupload.sequence == data && sem.take_nonblocking()) { fwupload.sequence++; fwupload.acked += fwupload.len; if (fwupload.acked == fwupload.length) { // trigger send of DATA16 ack to client fwupload.need_ack = true; } - sem->give(); + sem.give(); } break; } @@ -1030,7 +1028,7 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) if (fwupload.length != 0 && fwupload.length > fwupload.acked && ((fwupload.counter++ & 0x07) != 0) && - sem->take_nonblocking()) { + sem.take_nonblocking()) { pkt.type = fwupload.fw_type; pkt.payload.fw.seq = fwupload.sequence; uint32_t len = fwupload.length>fwupload.acked?fwupload.length - fwupload.acked:0; @@ -1043,7 +1041,7 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) pkt.payload.fw.offset, pkt.payload.fw.len, pkt.type); - sem->give(); + sem.give(); } else { pkt.type = TELEM_STATUS; pkt.payload.status = t_status; @@ -1070,7 +1068,7 @@ void AP_Radio_cc2500::send_SRT_telemetry(void) */ void AP_Radio_cc2500::check_fw_ack(void) { - if (fwupload.need_ack && sem->take_nonblocking()) { + if (fwupload.need_ack && sem.take_nonblocking()) { // ack the send of a DATA96 fw packet to TX fwupload.need_ack = false; uint8_t data16[16] {}; @@ -1078,7 +1076,7 @@ void AP_Radio_cc2500::check_fw_ack(void) memcpy(&data16[0], &ack_to, 4); mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); Debug(4,"sent ack DATA16\n"); - sem->give(); + sem.give(); } } diff --git a/libraries/AP_Radio/AP_Radio_cc2500.h b/libraries/AP_Radio/AP_Radio_cc2500.h index 66b1bf5b38..f4d1cbb28d 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.h +++ b/libraries/AP_Radio/AP_Radio_cc2500.h @@ -87,7 +87,7 @@ private: void radio_init(void); // semaphore between ISR and main thread - AP_HAL::Semaphore *sem; + HAL_Semaphore sem; AP_Radio::stats stats; AP_Radio::stats last_stats; diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index aa746603c5..038ea2c828 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -277,8 +277,6 @@ bool AP_Radio_cypress::init(void) #endif load_bind_info(); - sem = hal.util->new_semaphore(); - return reset(); } @@ -418,7 +416,7 @@ uint8_t AP_Radio_cypress::num_channels(void) void AP_Radio_cypress::check_fw_ack(void) { Debug(4,"check need_ack\n"); - if (fwupload.need_ack && sem->take_nonblocking()) { + if (fwupload.need_ack && sem.take_nonblocking()) { // ack the send of a DATA96 fw packet to TX fwupload.need_ack = false; uint8_t data16[16] {}; @@ -426,7 +424,7 @@ void AP_Radio_cypress::check_fw_ack(void) memcpy(&data16[0], &ack_to, 4); mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); Debug(4,"sent ack DATA16\n"); - sem->give(); + sem.give(); } } @@ -808,14 +806,14 @@ bool AP_Radio_cypress::parse_dsm_channels(const uint8_t *data) // got an ack from key 0 Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", v, fwupload.sequence, unsigned(fwupload.acked), unsigned(fwupload.length), fwupload.len); - if (fwupload.sequence == v && sem->take_nonblocking()) { + if (fwupload.sequence == v && sem.take_nonblocking()) { fwupload.sequence++; fwupload.acked += fwupload.len; if (fwupload.acked == fwupload.length) { // trigger send of DATA16 ack to client fwupload.need_ack = true; } - sem->give(); + sem.give(); } } if (chan == 7) { @@ -1596,7 +1594,7 @@ void AP_Radio_cypress::send_telem_packet(void) if (fwupload.length != 0 && fwupload.length > fwupload.acked && ((fwupload.counter++ & 0x07) != 0) && - sem->take_nonblocking()) { + sem.take_nonblocking()) { pkt.type = fwupload.fw_type; pkt.payload.fw.seq = fwupload.sequence; uint32_t len = fwupload.length>fwupload.acked?fwupload.length - fwupload.acked:0; @@ -1609,7 +1607,7 @@ void AP_Radio_cypress::send_telem_packet(void) pkt.payload.fw.offset, pkt.payload.fw.len, pkt.type); - sem->give(); + sem.give(); pkt.crc = crc_crc8((const uint8_t *)&pkt.type, 15); } else { pkt.type = TELEM_STATUS; @@ -1702,7 +1700,7 @@ void AP_Radio_cypress::handle_data_packet(mavlink_channel_t chan, const mavlink_ uint32_t ofs=0; memcpy(&ofs, &m.data[0], 4); Debug(4, "got data96 of len %u from chan %u at offset %u\n", m.len, chan, unsigned(ofs)); - if (sem->take_nonblocking()) { + if (sem.take_nonblocking()) { fwupload.chan = chan; fwupload.need_ack = false; fwupload.offset = ofs; @@ -1720,7 +1718,7 @@ void AP_Radio_cypress::handle_data_packet(mavlink_channel_t chan, const mavlink_ fwupload.fw_type = TELEM_FW; memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); } - sem->give(); + sem.give(); } } diff --git a/libraries/AP_Radio/AP_Radio_cypress.h b/libraries/AP_Radio/AP_Radio_cypress.h index c207c5f039..6a816219fa 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.h +++ b/libraries/AP_Radio/AP_Radio_cypress.h @@ -169,7 +169,7 @@ private: }; // semaphore between ISR and main thread - AP_HAL::Semaphore *sem; + HAL_Semaphore sem; // dsm config data and status struct {