mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
RcReceiver.cpp
[詳解]
1#include "RcReceiver.hpp"
2#include "hardware_config.hpp"
3
4volatile uint16_t RcReceiver::_pulse_width[MAX_CH];
5volatile uint32_t RcReceiver::_last_update[MAX_CH];
6volatile uint32_t RcReceiver::_rise_time[MAX_CH];
7uint8_t RcReceiver::_pins[MAX_CH];
8uint8_t RcReceiver::_num_ch = 0;
9
10void IRAM_ATTR RcReceiver::isr(void *arg) {
11 uint8_t ch = static_cast<uint8_t>(reinterpret_cast<uintptr_t>(arg));
12 if (ch >= _num_ch) {
13 return;
14 }
15 if (digitalRead(_pins[ch]) == HIGH) {
16 _rise_time[ch] = micros();
17 } else {
18 uint32_t width = micros() - _rise_time[ch];
19 // 有効なRCサーボパルス (RC_PULSE_MIN〜RC_PULSE_MAX) のみを受け入れる(ノイズ除去)
20 if (width >= RC_PULSE_MIN && width <= RC_PULSE_MAX) {
21 _pulse_width[ch] = static_cast<uint16_t>(width);
22 _last_update[ch] = millis();
23 }
24 }
25}
26
27void RcReceiver::begin(const uint8_t ch_pins[], uint8_t num_channels) {
28 _num_ch = (num_channels < MAX_CH) ? num_channels : MAX_CH;
29 for (uint8_t i = 0; i < _num_ch; i++) {
30 _pins[i] = ch_pins[i];
31 // 未接続時のフローティング防止。受信機接続時は受信機がラインを駆動する
32 pinMode(_pins[i], INPUT_PULLDOWN);
33 attachInterruptArg(digitalPinToInterrupt(_pins[i]), isr,
34 reinterpret_cast<void *>(static_cast<uintptr_t>(i)), CHANGE);
35 }
36}
37
38uint16_t RcReceiver::getPulseWidth(uint8_t channel) {
39 if (channel >= _num_ch) {
40 return 0;
41 }
42 noInterrupts();
43 uint16_t val = _pulse_width[channel];
44 interrupts();
45 return val;
46}
47
48bool RcReceiver::isSignalValid(uint8_t channel, uint32_t timeout_ms) {
49 if (channel >= _num_ch) {
50 return false;
51 }
52 noInterrupts();
53 uint32_t last = _last_update[channel];
54 interrupts();
55 return (last != 0) && ((millis() - last) < timeout_ms);
56}
57
58float RcReceiver::pulseToNormalized(uint16_t pulse_us) {
59 // 範囲外のパルス幅は 0 (停止) として処理
60 // スティック: 890〜2100で倒れ度合い、中立1496。SW: 1496→1995でON
61 if (pulse_us < RC_PULSE_MIN || pulse_us > RC_PULSE_MAX) {
62 return 0.0f;
63 }
64 if (abs(static_cast<int>(pulse_us) - RC_PULSE_MID) < RC_PULSE_DEADZONE) {
65 return 0.0f;
66 }
67 float norm = (static_cast<float>(pulse_us) - RC_PULSE_MID) / ((RC_PULSE_MAX - RC_PULSE_MIN) / 2.0f);
68 return constrain(norm, -1.0f, 1.0f);
69}
プロポ受信機のパルス幅を計測し、中立をゼロとする操作量に変換する。
void begin(const uint8_t ch_pins[], uint8_t num_channels)
uint16_t getPulseWidth(uint8_t channel)
static float pulseToNormalized(uint16_t pulse_us)
bool isSignalValid(uint8_t channel, uint32_t timeout_ms)
static constexpr uint8_t MAX_CH
ESP32のピン、RC信号の範囲、速度上限と周期を定義する。
#define RC_PULSE_MID
#define RC_PULSE_MIN
#define RC_PULSE_DEADZONE
#define RC_PULSE_MAX