-
Notifications
You must be signed in to change notification settings - Fork 8
/
receiver.cpp
179 lines (153 loc) · 7.47 KB
/
receiver.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
/*
* Flybrix Flight Controller -- Copyright 2018 Flying Selfie Inc. d/b/a Flybrix
*
* http://www.flybrix.com
*/
#include "receiver.h"
#include "board.h"
#include "debug.h"
// RX -- PKZ3341 sends: RHS left/right, RHS up/down, LHS up/down, LHS
// left/right, RHS click (latch), LHS button(momentary)
// map throttle to LHS up/down
// map pitch to RHS up/down
// map roll to RHS left/righ
// map yaw to LHS up/down
// map AUX1 to RHS click
// map AUX2 to LHS click
Receiver::ChannelProperties::ChannelProperties() : assignment{2, 1, 0, 3, 4, 5}, inversion{6}, midpoint{1500, 1500, 1500, 1500, 1500, 1500}, deadzone{0, 0, 0, 20, 0, 0} {
}
volatile uint8_t RX_freshness = 0;
volatile uint16_t RX[RC_CHANNEL_COUNT]; // filled by the interrupt with valid data
volatile uint16_t RX_errors = 0; // count dropped frames
volatile uint16_t startPulse = 0; // keeps track of the last received pulse position
volatile uint16_t RX_buffer[RC_CHANNEL_COUNT]; // buffer data in anticipation of a valid frame
volatile uint8_t RX_channel = 0; // we are collecting data for this channel
bool Receiver::ChannelProperties::verify() const {
bool ok{true};
bool assigned[] = {false, false, false, false, false, false};
for (size_t idx = 0; idx < 6; ++idx) {
uint8_t assig = assignment[idx];
if (assig < 0 || assig > 5) {
ok = false;
DebugPrint("All channel assignments must be within the [0, 5] range");
} else if (assigned[assig]) {
ok = false;
DebugPrint("Duplicate channel assignment detected");
} else {
assigned[assig] = true;
}
}
for (size_t idx = 0; idx < 6; ++idx) {
if (midpoint[idx] < PPMchannel::min || midpoint[idx] > PPMchannel::max) {
ok = false;
DebugPrint("Channel midpoints must be within the channel range");
}
if (midpoint[idx] < deadzone[idx]) {
ok = false;
DebugPrint("Channel deadzone cannot be larger than the midpoint value");
}
}
return ok;
}
Receiver::Receiver() {
pinMode(board::RX_DAT, INPUT); // WE ARE ASSUMING RX_DAT IS PIN 3 IN FTM1 SETUP!
// FLEX Timer1 input filter configuration
// 4+4*val clock cycles, 48MHz = 4+4*7 = 32 clock cycles = 0.75us
FTM1_FILTER = 0x07;
// FLEX Timer1 configuration
SIM_SCGC6 |= SIM_SCGC6_FTM1; // Enable FTM1 clock
FTM1_SC = 0x0C; // TOF=0 TOIE=0 CPWMS=0 CLKS=01 (system clock) PS=100 (divide by 16)
FTM1_MOD = 0xFFFF; // modulo to max
FTM1_C0SC = 0x44; // CHF=0 CHIE=1 MSB=0 MSA=0 ELSB=0 ELSA=1 DMA=0
// Enable FTM1 interrupt inside NVIC
NVIC_ENABLE_IRQ(IRQ_FTM1);
// PIN configuration, alternative function 3
PORTA_PCR12 |= 0x300; // Signal sampling is being done via PORTA_PCR12 (PIN 3).
}
extern "C" void ftm1_isr(void) {
// save current interrupt count/time
uint16_t stopPulse = FTM1_C0V;
// clear channel interrupt flag (CHF)
FTM1_C0SC &= ~0x80;
uint16_t pulseWidth = stopPulse - startPulse;
// Error / Sanity check
// too short to be data : pulseWidth < 900us
// between data and sync : (pulseWidth > 2100us and pulseWidth < RX_PPM_SYNCPULSE_MIN)
// too long : pulseWidth > RX_PPM_SYNCPULSE_MAX;
if (pulseWidth < 2700 || (pulseWidth > 6300 && pulseWidth < RX_PPM_SYNCPULSE_MIN) || pulseWidth > RX_PPM_SYNCPULSE_MAX) {
RX_errors++;
RX_channel = RC_CHANNEL_COUNT + 1; // set RX_channel out of range to drop frame
} else if (pulseWidth > RX_PPM_SYNCPULSE_MIN) { // this is the sync pulse
if (RX_channel <= RC_CHANNEL_COUNT) { // valid frame = push from our buffer
for (uint8_t i = 0; i < RC_CHANNEL_COUNT; i++) {
RX_freshness = 20;
RX[i] = RX_buffer[i];
}
}
// restart the channel counter whether the frame was valid or not
RX_channel = 0;
} else { // this pulse is channel data
if (RX_channel < RC_CHANNEL_COUNT) { // extra channels will get ignored here
RX_buffer[RX_channel] = pulseWidth / 3; // Store measured pulse length in usec
RX_channel++; // Advance to next channel
}
}
startPulse = stopPulse; // Save time at pulse start
}
RcState Receiver::query() {
RcState rc_state;
cli(); // disable interrupts
bool input_is_ready = (RX_freshness > 0);
if (input_is_ready) {
--RX_freshness;
// if receiver is working, we should never see anything less than 900!
for (uint8_t i = 0; i < RC_CHANNEL_COUNT; i++) {
if (RX[i] < 900) {
input_is_ready = false;
break;
}
}
}
if (!input_is_ready) {
// tell state that Receiver is not ready and return
sei(); // enable interrupts
rc_state.status = RcStatus::Timeout;
return rc_state;
}
// read data into PPMchannel objects using receiver channels assigned from configuration
throttle.val = RX[channel.assignment[0]];
pitch.val = RX[channel.assignment[1]];
roll.val = RX[channel.assignment[2]];
yaw.val = RX[channel.assignment[3]];
AUX1.val = RX[channel.assignment[4]];
AUX2.val = RX[channel.assignment[5]];
// update midpoints from config
throttle.mid = channel.midpoint[channel.assignment[0]];
pitch.mid = channel.midpoint[channel.assignment[1]];
roll.mid = channel.midpoint[channel.assignment[2]];
yaw.mid = channel.midpoint[channel.assignment[3]];
AUX1.mid = channel.midpoint[channel.assignment[4]];
AUX2.mid = channel.midpoint[channel.assignment[5]];
// update deadzones from config
throttle.deadzone = channel.deadzone[channel.assignment[0]];
pitch.deadzone = channel.deadzone[channel.assignment[1]];
roll.deadzone = channel.deadzone[channel.assignment[2]];
yaw.deadzone = channel.deadzone[channel.assignment[3]];
AUX1.deadzone = channel.deadzone[channel.assignment[4]];
AUX2.deadzone = channel.deadzone[channel.assignment[5]];
sei(); // enable interrupts
// Translate PPMChannel data into the four command level and aux mask
rc_state.status = RcStatus::Ok;
rc_state.command.parseBools(AUX1.isLow(), AUX1.isMid(), AUX1.isHigh(), AUX2.isLow(), AUX2.isMid(), AUX2.isHigh());
// in some cases it is impossible to get a ppm channel to be close enought to the midpoint (~1500 usec) because the controller trim is too coarse to correct a small error
// we get around this by creating a small dead zone around the midpoint of signed channel, specified in usec units
pitch.val = pitch.applyDeadzone();
roll.val = roll.applyDeadzone();
yaw.val = yaw.applyDeadzone();
uint16_t throttle_threshold = ((throttle.max - throttle.min) / 10) + throttle.min; // keep bottom 10% of throttle stick to mean 'off'
rc_state.command.throttle = constrain((throttle.val - throttle_threshold) * 4095 / (throttle.max - throttle_threshold), 0, 4095);
rc_state.command.pitch = constrain((1 - 2 * ((channel.inversion >> 1) & 1)) * (pitch.val - pitch.mid) * 4095 / (pitch.max - pitch.min), -2047, 2047);
rc_state.command.roll = constrain((1 - 2 * ((channel.inversion >> 2) & 1)) * (roll.val - roll.mid) * 4095 / (roll.max - roll.min), -2047, 2047);
rc_state.command.yaw = constrain((1 - 2 * ((channel.inversion >> 3) & 1)) * (yaw.val - yaw.mid) * 4095 / (yaw.max - yaw.min), -2047, 2047);
return rc_state;
}