mirror of
https://github.com/brendanhaines/RasPi.git
synced 2024-09-19 19:55:50 -06:00
35 lines
903 B
C++
35 lines
903 B
C++
|
#include "v2DSM2.h"
|
||
|
|
||
|
#include <iostream>
|
||
|
#include "wiringSerial.h"
|
||
|
|
||
|
#define VALUE_MASK 0x3FF
|
||
|
|
||
|
v2DSM2::v2DSM2( int mode )
|
||
|
{
|
||
|
std::cout << "opening serial port..." << std::flush;
|
||
|
fd = serialOpen( "/dev/ttyAMA0", 115200 );
|
||
|
std::cout << "SUCCESS " << fd << std::endl;
|
||
|
std::cout << "flushing serial..." << std::flush;
|
||
|
serialFlush(fd);
|
||
|
std::cout << "SUCCESS" << std::endl;
|
||
|
}
|
||
|
|
||
|
void v2DSM2::update()
|
||
|
{
|
||
|
int i = 0;
|
||
|
|
||
|
if( serialDataAvail(fd) < 16 ) return;
|
||
|
|
||
|
for( i = 0; i < 8; i++ )
|
||
|
data[ i ] = ( (int)(serialGetchar(fd)) << 8 ) + serialGetchar(fd);
|
||
|
|
||
|
*(values + 0) = data[7] & VALUE_MASK; //Throttle
|
||
|
*(values + 1) = data[1] & VALUE_MASK; //Aileron
|
||
|
*(values + 2) = data[3] & VALUE_MASK; //Elevator
|
||
|
*(values + 3) = data[5] & VALUE_MASK; //Rudder
|
||
|
*(values + 4) = data[4] & VALUE_MASK; //Gear
|
||
|
*(values + 5) = data[2] & VALUE_MASK; //Aux1
|
||
|
return;
|
||
|
}
|