124 lines
3.1 KiB
C++
124 lines
3.1 KiB
C++
#ifndef _RC_H_
|
|
#define _RC_H_
|
|
|
|
|
|
#define RC
|
|
|
|
#include <PulsePosition.h>
|
|
PulsePositionInput ppmIn;
|
|
|
|
#define PPMREADPERIOD 20 //period for d4r-ii is 18ms
|
|
static int count=0;
|
|
|
|
#define PIN_PPM 9
|
|
|
|
#define PPM_CHANNELS 8
|
|
#define PPM_SAVE_MIN 950 //minimum allowed for valid data
|
|
#define PPM_SAVE_MAX 2050 //maximum allowed for valid data
|
|
#define PPM_MIN 1000
|
|
#define PPM_MAX 2000
|
|
#define PPM_MID 1500
|
|
#define PPM_DEADBAND 5
|
|
#define PPM_CHANNEL_THROTTLE 1
|
|
#define PPM_CHANNEL_STEER 2
|
|
#define PPM_CHANNEL_MODE 5
|
|
float ppm[PPM_CHANNELS];
|
|
|
|
unsigned long ppm_count_success=0;
|
|
unsigned long ppm_count_fail=0;
|
|
|
|
void initRC();
|
|
void readPPM(unsigned long loopmillis);
|
|
void printPPM();
|
|
|
|
int16_t getRCThrottle();
|
|
int16_t getRCSteer();
|
|
uint8_t getRCMode(uint8_t modes);
|
|
|
|
bool rc_enabled=false;
|
|
|
|
void initRC() {
|
|
ppmIn.begin(PIN_PPM);
|
|
}
|
|
|
|
void readPPM(unsigned long loopmillis) {
|
|
static unsigned long ppm_last_received=0;
|
|
static unsigned long ppm_last_received_success=0;
|
|
int ppm_num = ppmIn.available();
|
|
if (ppm_num > 0) {
|
|
bool ppm_ok=true;
|
|
ppm_last_received=loopmillis;
|
|
float new_ppm[PPM_CHANNELS];
|
|
if (ppm_num != PPM_CHANNELS) {
|
|
ppm_ok=false;
|
|
}else{
|
|
for (int i=1; i <= ppm_num; i++) {
|
|
float val=ppmIn.read(i);
|
|
new_ppm[i-1]=val;
|
|
if (val<PPM_SAVE_MIN || val>PPM_SAVE_MAX) {
|
|
ppm_ok=false;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (ppm_ok) {
|
|
ppm_count_success+=1;
|
|
ppm_last_received_success=loopmillis;
|
|
std::copy(std::begin(new_ppm), std::end(new_ppm), std::begin(ppm));
|
|
}else{
|
|
ppm_count_fail+=1;
|
|
}
|
|
}
|
|
}
|
|
|
|
float getPPMSuccessrate() {
|
|
float rate=ppm_count_success*1.0/(ppm_count_success+ppm_count_fail);
|
|
if (ppm_count_success+ppm_count_fail>500) {
|
|
ppm_count_success=0;
|
|
ppm_count_fail=0;
|
|
}
|
|
return rate;
|
|
}
|
|
|
|
//Return value 0 - 1000
|
|
int16_t getRCThrottle() {
|
|
return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
|
|
}
|
|
|
|
int16_t getRCBrake() {
|
|
return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,1000), 0,1000);
|
|
}
|
|
|
|
//Return value -1000 - 1000
|
|
int16_t getRCSteer() {
|
|
if (ppm[PPM_CHANNEL_STEER-1]>PPM_MID+PPM_DEADBAND){
|
|
return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
|
|
}else if (ppm[PPM_CHANNEL_STEER-1]<PPM_MID-PPM_DEADBAND){
|
|
return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,-1000), -1000,0);
|
|
}else{
|
|
return 0;
|
|
}
|
|
|
|
}
|
|
|
|
//Return descrete value for equidistant ranges. modes = number of ranges.
|
|
uint8_t getRCMode(uint8_t modes) {
|
|
float v=constrain(map(ppm[PPM_CHANNEL_MODE-1],PPM_MIN,PPM_MAX,0.0,1.0), 0.0,0.999);
|
|
return uint8_t(v*modes);
|
|
}
|
|
|
|
|
|
void printPPM(unsigned long loopmillis){
|
|
|
|
count = count + 1;
|
|
Serial.print("ms :");
|
|
Serial.print(count);
|
|
Serial.print(" : ");
|
|
for (int i=1; i <= PPM_CHANNELS; i++) {
|
|
Serial.print(ppm[i-1]);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.println();
|
|
}
|
|
|
|
#endif |