Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: implement cork and push for remaining RCOutput backends

Browse Source
This commit is contained in:
Andrew Tridgell 2016-10-12 11:19:16 +11:00
parent c8179a0887
commit 7f09d20e3b
6 changed files with 72 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_HAL_Linux/RCOutput_PRU.cpp
View File

@ -68,7 +68,12 @@ void RCOutput_PRU::disable_ch(uint8_t ch)
void RCOutput_PRU::write(uint8_t ch, uint16_t period_us)
{
if (corked) {
pending[ch] = period_us;
pending_mask |= (1U << ch);
} else {
sharedMem_cmd->periodhi[chan_pru_map[ch]][1] = TICK_PER_US*period_us;
}
}
uint16_t RCOutput_PRU::read(uint8_t ch)
@ -86,3 +91,19 @@ void RCOutput_PRU::read(uint16_t* period_us, uint8_t len)
period_us[i] = sharedMem_cmd->hilo_read[chan_pru_map[i]][1]/TICK_PER_US;
}
}
void RCOutput_PRU::cork(void)
{
corked = true;
}
void RCOutput_PRU::push(void)
{
corked = false;
for (uint8_t i=0; i<ARRAY_SIZE(pending); i++) {
if (pending_mask & (1U << i)) {
write(i, pending[i]);
}
}
pending_mask = 0;
}

6
libraries/AP_HAL_Linux/RCOutput_PRU.h
View File

@ -24,6 +24,8 @@ class RCOutput_PRU : public AP_HAL::RCOutput {
void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void cork(void) override;
void push(void) override;
private:
static const int TICK_PER_US=200;
@ -38,6 +40,8 @@ private:
};
volatile struct pwm_cmd *sharedMem_cmd;
uint16_t pending[MAX_PWMS];
bool corked;
uint32_t pending_mask;
};
}

21
libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp
View File

@ -76,7 +76,12 @@ void RCOutput_ZYNQ::disable_ch(uint8_t ch)
void RCOutput_ZYNQ::write(uint8_t ch, uint16_t period_us)
{
if (corked) {
pending[ch] = period_us;
pending_mask |= (1U << ch);
} else {
sharedMem_cmd->periodhi[ch].hi = TICK_PER_US*period_us;
}
}
uint16_t RCOutput_ZYNQ::read(uint8_t ch)
@ -94,3 +99,19 @@ void RCOutput_ZYNQ::read(uint16_t* period_us, uint8_t len)
period_us[i] = sharedMem_cmd->periodhi[i].hi/TICK_PER_US;
}
}
void RCOutput_ZYNQ::cork(void)
{
corked = true;
}
void RCOutput_ZYNQ::push(void)
{
corked = false;
for (uint8_t i=0; i<MAX_ZYNQ_PWMS; i++) {
if (pending_mask & (1U << i)) {
write(i, pending[i]);
}
}
pending_mask = 0;
}

6
libraries/AP_HAL_Linux/RCOutput_ZYNQ.h
View File

@ -16,6 +16,8 @@ public:
void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void cork(void) override;
void push(void) override;
private:
static const int TICK_PER_US=100;
@ -30,6 +32,10 @@ private:
struct s_period_hi periodhi[MAX_ZYNQ_PWMS];
};
volatile struct pwm_cmd *sharedMem_cmd;
uint16_t pending[MAX_ZYNQ_PWMS];
bool corked;
uint32_t pending_mask;
};
}

13
libraries/AP_HAL_Linux/RCOutput_qflight.cpp
View File

@ -76,7 +76,9 @@ void RCOutput_QFLIGHT::write(uint8_t ch, uint16_t period_us)
return;
}
period[ch] = period_us;
if (!corked) {
need_write = true;
}
}
uint16_t RCOutput_QFLIGHT::read(uint8_t ch)
@ -172,5 +174,16 @@ void RCOutput_QFLIGHT::check_rc_in(void)
}
}
void RCOutput_QFLIGHT::cork(void)
{
corked = true;
}
void RCOutput_QFLIGHT::push(void)
{
corked = false;
need_write = true;
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

3
libraries/AP_HAL_Linux/RCOutput_qflight.h
View File

@ -18,6 +18,8 @@ public:
uint16_t read(uint8_t ch);
void read(uint16_t *period_us, uint8_t len);
void set_device_path(const char *device);
void cork(void) override;
void push(void) override;
private:
const char *device = nullptr;
@ -44,6 +46,7 @@ private:
uint8_t bytes[19];
} rcu;
uint8_t nrcin_bytes;
bool corked;
};
}