73 lines
1.2 KiB
C++
73 lines
1.2 KiB
C++
// File: TrackFPV.ino
|
|
// Date: 15.11.2016
|
|
// Author: Hendrik Schutter
|
|
// Version: a0.1
|
|
|
|
#include <Joystick.h>
|
|
#include <CPPM.h>
|
|
|
|
int yawRaw;
|
|
int pitchRaw;
|
|
int min = -500;
|
|
int max = 500;
|
|
|
|
void cppm_cycle(void){
|
|
if (CPPM.synchronized()){
|
|
yawRaw = CPPM.read_us(CPPM_GEAR) - 1500;
|
|
pitchRaw = CPPM.read_us(CPPM_AUX1) - 1500;
|
|
}
|
|
else
|
|
{
|
|
Serial.println("Nix");
|
|
}
|
|
}
|
|
|
|
int getYAW() {
|
|
yawRaw = yawRaw - 22;
|
|
yawRaw = max_min(yawRaw);
|
|
float f = yawRaw/5;
|
|
return f;
|
|
}
|
|
|
|
int getPITCH() {
|
|
pitchRaw = pitchRaw - 22;
|
|
pitchRaw = max_min(pitchRaw);
|
|
float f = pitchRaw/5;
|
|
return f;
|
|
}
|
|
|
|
int max_min(int zahl) {
|
|
if (zahl > max) {
|
|
zahl = max;
|
|
}
|
|
if (zahl < min) {
|
|
zahl = min;
|
|
}
|
|
return zahl;
|
|
}
|
|
|
|
|
|
void setup(){
|
|
Serial.begin(9600);
|
|
CPPM.begin();
|
|
Joystick.begin(true);
|
|
}
|
|
|
|
void loop(){
|
|
cppm_cycle();
|
|
Serial.print("Pitchraw: ");
|
|
Serial.println(pitchRaw-22);
|
|
Serial.print("YAWraw: ");
|
|
Serial.println(yawRaw-22);
|
|
|
|
Serial.print("Pitch: ");
|
|
Serial.println(getPITCH());
|
|
Serial.print("YAW: ");
|
|
Serial.println(getYAW());
|
|
|
|
Joystick.setYAxis(getPITCH());
|
|
Joystick.setXAxis(getYAW());
|
|
|
|
delay(100);
|
|
}
|