aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Radio: use WITH_SEMAPHORE()

Browse Source 
and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent 17049ff5f5
commit 68f463b29d
4 changed files with 36 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Radio/AP_Radio_cc2500.cpp
View File

@ -68,8 +68,6 @@ bool AP_Radio_cc2500::init(void)
irq_handler_thd, /* Thread function. */ irq_handler_thd, /* Thread function. */
NULL); /* Thread parameter. */ NULL); /* Thread parameter. */
#endif #endif
sem = hal.util->new_semaphore();
return reset(); return reset();
} }
@ -340,7 +338,9 @@ void AP_Radio_cc2500::handle_data_packet(mavlink_channel_t chan, const mavlink_d
uint32_t ofs=0; uint32_t ofs=0;
memcpy(&ofs, &m.data[0], 4); 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)); Debug(4, "got data96 of len %u from chan %u at offset %u\n", m.len, chan, unsigned(ofs));
if (sem->take_nonblocking()) {
WITH_SEMAPHORE(sem);
fwupload.chan = chan; fwupload.chan = chan;
fwupload.need_ack = false; fwupload.need_ack = false;
fwupload.offset = ofs; fwupload.offset = ofs;
@ -358,8 +358,6 @@ void AP_Radio_cc2500::handle_data_packet(mavlink_channel_t chan, const mavlink_d
fwupload.fw_type = TELEM_FW; fwupload.fw_type = TELEM_FW;
memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length);
} }
sem->give();
}
} }
/* /*
@ -462,14 +460,14 @@ bool AP_Radio_cc2500::handle_SRT_packet(const uint8_t *packet)
// got an fw upload ack // got an fw upload ack
Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n",
data, fwupload.sequence, unsigned(fwupload.acked), unsigned(fwupload.length), fwupload.len); 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.sequence++;
fwupload.acked += fwupload.len; fwupload.acked += fwupload.len;
if (fwupload.acked == fwupload.length) { if (fwupload.acked == fwupload.length) {
// trigger send of DATA16 ack to client // trigger send of DATA16 ack to client
fwupload.need_ack = true; fwupload.need_ack = true;
} }
sem->give(); sem.give();
} }
break; break;
} }
@ -1030,7 +1028,7 @@ void AP_Radio_cc2500::send_SRT_telemetry(void)
if (fwupload.length != 0 && if (fwupload.length != 0 &&
fwupload.length > fwupload.acked && fwupload.length > fwupload.acked &&
((fwupload.counter++ & 0x07) != 0) && ((fwupload.counter++ & 0x07) != 0) &&
sem->take_nonblocking()) { sem.take_nonblocking()) {
pkt.type = fwupload.fw_type; pkt.type = fwupload.fw_type;
pkt.payload.fw.seq = fwupload.sequence; pkt.payload.fw.seq = fwupload.sequence;
uint32_t len = fwupload.length>fwupload.acked?fwupload.length - fwupload.acked:0; 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.offset,
pkt.payload.fw.len, pkt.payload.fw.len,
pkt.type); pkt.type);
sem->give(); sem.give();
} else { } else {
pkt.type = TELEM_STATUS; pkt.type = TELEM_STATUS;
pkt.payload.status = t_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) 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 // ack the send of a DATA96 fw packet to TX
fwupload.need_ack = false; fwupload.need_ack = false;
uint8_t data16[16] {}; uint8_t data16[16] {};
@ -1078,7 +1076,7 @@ void AP_Radio_cc2500::check_fw_ack(void)
memcpy(&data16[0], &ack_to, 4); memcpy(&data16[0], &ack_to, 4);
mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); mavlink_msg_data16_send(fwupload.chan, 42, 4, data16);
Debug(4,"sent ack DATA16\n"); Debug(4,"sent ack DATA16\n");
sem->give(); sem.give();
} }
} }

2
libraries/AP_Radio/AP_Radio_cc2500.h
View File

@ -87,7 +87,7 @@ private:
void radio_init(void); void radio_init(void);
// semaphore between ISR and main thread // semaphore between ISR and main thread
AP_HAL::Semaphore *sem; HAL_Semaphore sem;
AP_Radio::stats stats; AP_Radio::stats stats;
AP_Radio::stats last_stats; AP_Radio::stats last_stats;

18
libraries/AP_Radio/AP_Radio_cypress.cpp
View File

@ -277,8 +277,6 @@ bool AP_Radio_cypress::init(void)
#endif #endif
load_bind_info(); load_bind_info();
sem = hal.util->new_semaphore();
return reset(); return reset();
} }
@ -418,7 +416,7 @@ uint8_t AP_Radio_cypress::num_channels(void)
void AP_Radio_cypress::check_fw_ack(void) void AP_Radio_cypress::check_fw_ack(void)
{ {
Debug(4,"check need_ack\n"); 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 // ack the send of a DATA96 fw packet to TX
fwupload.need_ack = false; fwupload.need_ack = false;
uint8_t data16[16] {}; uint8_t data16[16] {};
@ -426,7 +424,7 @@ void AP_Radio_cypress::check_fw_ack(void)
memcpy(&data16[0], &ack_to, 4); memcpy(&data16[0], &ack_to, 4);
mavlink_msg_data16_send(fwupload.chan, 42, 4, data16); mavlink_msg_data16_send(fwupload.chan, 42, 4, data16);
Debug(4,"sent ack DATA16\n"); 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 // got an ack from key 0
Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n", Debug(4, "ack %u seq=%u acked=%u length=%u len=%u\n",
v, fwupload.sequence, unsigned(fwupload.acked), unsigned(fwupload.length), fwupload.len); 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.sequence++;
fwupload.acked += fwupload.len; fwupload.acked += fwupload.len;
if (fwupload.acked == fwupload.length) { if (fwupload.acked == fwupload.length) {
// trigger send of DATA16 ack to client // trigger send of DATA16 ack to client
fwupload.need_ack = true; fwupload.need_ack = true;
} }
sem->give(); sem.give();
} }
} }
if (chan == 7) { if (chan == 7) {
@ -1596,7 +1594,7 @@ void AP_Radio_cypress::send_telem_packet(void)
if (fwupload.length != 0 && if (fwupload.length != 0 &&
fwupload.length > fwupload.acked && fwupload.length > fwupload.acked &&
((fwupload.counter++ & 0x07) != 0) && ((fwupload.counter++ & 0x07) != 0) &&
sem->take_nonblocking()) { sem.take_nonblocking()) {
pkt.type = fwupload.fw_type; pkt.type = fwupload.fw_type;
pkt.payload.fw.seq = fwupload.sequence; pkt.payload.fw.seq = fwupload.sequence;
uint32_t len = fwupload.length>fwupload.acked?fwupload.length - fwupload.acked:0; 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.offset,
pkt.payload.fw.len, pkt.payload.fw.len,
pkt.type); pkt.type);
sem->give(); sem.give();
pkt.crc = crc_crc8((const uint8_t *)&pkt.type, 15); pkt.crc = crc_crc8((const uint8_t *)&pkt.type, 15);
} else { } else {
pkt.type = TELEM_STATUS; 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; uint32_t ofs=0;
memcpy(&ofs, &m.data[0], 4); 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)); 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.chan = chan;
fwupload.need_ack = false; fwupload.need_ack = false;
fwupload.offset = ofs; 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; fwupload.fw_type = TELEM_FW;
memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length); memcpy(&fwupload.pending_data[0], &m.data[4], fwupload.length);
} }
sem->give(); sem.give();
} }
} }

2
libraries/AP_Radio/AP_Radio_cypress.h
View File

@ -169,7 +169,7 @@ private:
}; };
// semaphore between ISR and main thread // semaphore between ISR and main thread
AP_HAL::Semaphore *sem; HAL_Semaphore sem;
// dsm config data and status // dsm config data and status
struct { struct {