Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
/* big thanks for Xorgon from #cleanflight irc channel for this code */
#include <SoftwareSerial.h>
#define MSP_ATTITUDE 108
SoftwareSerial mspSerial(11, 12); // RX TX
// PPM stuff
#define chanel_number 2 //set the number of chanels
#define default_servo_value 1500 //set the default servo value
#define PPM_FrLen 22500 //set the PPM frame length in microseconds (1ms = 1000µs)
#define PPM_PulseLen 300 //set the pulse length
#define onState 1 //set polarity of the pulses: 1 is positive, 0 is negative
#define sigPin 10 //set PPM signal output pin on the arduino
/*this array holds the servo values for the ppm signal
change theese values in your code (usually servo values move between 1000 and 2000)*/
int ppm[chanel_number];
void setup() {
/* PPM stuff */
for(int i=0; i<chanel_number; i++){
ppm[i]= default_servo_value;
}
pinMode(sigPin, OUTPUT);
digitalWrite(sigPin, !onState); //set the PPM signal pin to the default state (off)
cli();
TCCR1A = 0; // set entire TCCR1 register to 0
TCCR1B = 0;
OCR1A = 100; // compare match register, change this
TCCR1B |= (1 << WGM12); // turn on CTC mode
TCCR1B |= (1 << CS11); // 8 prescaler: 0,5 microseconds at 16mhz
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
sei();
/* /PPM */
mspSerial.begin(115200);
Serial.begin(115200);
}
void loop() {
/* PPM */
static int val = 1;
ppm[0] = ppm[0] + val;
if(ppm[0] >= 2000){ val = -1; }
if(ppm[0] <= 1000){ val = 1; }
delay(10);
/* /PPM */
uint8_t datad = 0;
uint8_t *data = &datad;
sendMSP(MSP_ATTITUDE, data, 0);
readData();
delay(100);
}
void sendMSP(uint8_t cmd, uint8_t *data, uint8_t n_bytes) {
uint8_t checksum = 0;
mspSerial.write((byte *) "$M<", 3);
mspSerial.write(n_bytes);
checksum ^= n_bytes;
mspSerial.write(cmd);
checksum ^= cmd;
mspSerial.write(checksum);
}
void readData() {
delay(100);
byte count = 0;
int16_t roll;
int16_t pitch;
int16_t yaw;
while (mspSerial.available()) {
count += 1;
byte c = mspSerial.read();
switch (count) {
case 6:
roll = c;
break;
case 7:
roll <<= 8;
roll += c;
roll = (roll & 0xFF00) >> 8 | (roll & 0x00FF) << 8; // Reverse the order of bytes
break;
case 8:
pitch += c;
break;
case 9:
pitch <<= 8;
pitch += c;
pitch = (pitch & 0xFF00) >> 8 | (pitch & 0x00FF) << 8; // Reverse the order of bytes
break;
case 10:
yaw += c;
break;
case 11:
yaw <<= 8;
yaw += c;
yaw = (yaw & 0xFF00) >> 8 | (yaw & 0x00FF) << 8; // Reverse the order of bytes
break;
}
}
Serial.print("Roll: " + String(roll/10.0));
Serial.print(" Pitch: " + String(pitch/10.0));
Serial.println(" Yaw: " + String(yaw));
ppm[2] = pitch/10.0;
ppm[1] = yaw;
}
/* PPM function */
ISR(TIMER1_COMPA_vect){ //leave this alone
static boolean state = true;
TCNT1 = 0;
if(state) { //start pulse
digitalWrite(sigPin, onState);
OCR1A = PPM_PulseLen * 2;
state = false;
}
else{ //end pulse and calculate when to start the next pulse
static byte cur_chan_numb;
static unsigned int calc_rest;
digitalWrite(sigPin, !onState);
state = true;
if(cur_chan_numb >= chanel_number){
cur_chan_numb = 0;
calc_rest = calc_rest + PPM_PulseLen;//
OCR1A = (PPM_FrLen - calc_rest) * 2;
calc_rest = 0;
}
else{
OCR1A = (ppm[cur_chan_numb] - PPM_PulseLen) * 2;
calc_rest = calc_rest + ppm[cur_chan_numb];
cur_chan_numb++;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment