Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues 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

54
libraries/AP_Radio/AP_Radio_cc2500.cpp
View File

@ -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();
}
}

2
libraries/AP_Radio/AP_Radio_cc2500.h
View File

@ -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;

18
libraries/AP_Radio/AP_Radio_cypress.cpp
View File

@ -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();
}
}

2
libraries/AP_Radio/AP_Radio_cypress.h
View File

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