Good evening,
I have a radio commanded quadcopter
[
]
http://www.asctec.de/downloads/researchpilot_manual.pdf[
^]
which could be controlled via PC using a serial communication protocol.
The protocol format is a 18 byte string built as follow:
1)startstring 1: > (0x3e in hex)
2)startstring 2: * ( 0x2A)
3)startstring 3: > (0x3e in hex)
4)packet descripotr -> always 0x17
5)length of packet - lowbyte
6)length of packet - highbyte
7)pitch - low byte
8)pitch - high byte
9)roll -low byte
10)roll - high byte
11)thrust - low byte
12)thrust - high byte
13)yaw - low byte
14)yaw - high byte
15)flags -low byte --->> every single bit of this one enable the communication between serial port and a parts of my copter (epecially, an 1 in in 8th the third position from left , as 1000 0100 (0x84 in hex) enable comm via serial interface and enable thrust control through serial interface.
16)flags - high byte --> sendrate of packet
17)crc16 - high
18)crc16- low
Using a VC++ console program I made (with MS visual C++ express 2010 on windows 7 x32), I can send a correct packet, as I'm receiving a byte from the copter that is 0x19, written in the guide as the received packet descriptor.
The problem is that I can't "move" it, and I'm putting positive values for thrust and a 0x84 for flags.
I've also a testsoftware provided from Asctec that works, can start engine and increase thrust. I've a pair of .avi using a serial monitor, one of this software and one of my code, If someone would take a look they are uploaded in megaupload here:
testsoftware that works
[url]
http://www.megaupload.com/?d=2SZ4Z4HQ[
^][/url]
my code, not working
[url]
http://www.megaupload.com/?d=K8R9IIHX[
^][/url]
I think that my problem should be some rigidities in write and read methods, I'm just sending the same packet in a while cycle.
If someone could address me to some notions. It would be a great favour!!
Thanks a lot, my program is the following:
namespace sendPacket {
#include "stdafx.h"
#include <conio.h>
#include <cstdio>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <stdexcept>
#include <valarray>
using namespace System;
using namespace std;
using namespace System::IO;
using namespace System::IO::Ports;
using namespace System::Threading;
struct FLAGS
{
unsigned char pitch:1;
unsigned char roll:2;
unsigned char thrust:3;
unsigned char yaw:4;
unsigned char acc:5;
unsigned char height:6;
unsigned char acc_height:7;
unsigned char trigger:8;
unsigned char freq;
};
struct SCIENTIFIC_COMMANDDATA
{
unsigned char packetdescriptor;
unsigned short pitch;
unsigned short roll;
unsigned short thrust;
unsigned short yaw;
FLAGS flags;
};
void sendData_String(array<unsigned char> ^ send_data)
{
StringComparer^ stringComparer = StringComparer::OrdinalIgnoreCase;
SerialPort^ _serialPort = gcnew SerialPort("COM3");
_serialPort->BaudRate = 9600 ;
_serialPort->Parity = Parity::None;
_serialPort->DataBits = 8;
_serialPort->StopBits = StopBits::One;
_serialPort->Handshake =Handshake::None;
array<unsigned char> ^ received_data =gcnew array<unsigned char>(50);
_serialPort->Open();
while(!_kbhit())
{
printf("\nsending data...\n");
for(int ii=0;ii<=send_data->Length;ii++)
{
Thread::Sleep(50);
_serialPort->Write(send_data,ii,1);
printf("%d", send_data[ii]);
Thread::Sleep(50);
}
printf("\nreceiving data...\n");
for (int ii=0;ii<=received_data->Length; ii++)
{
_serialPort->Read(received_data,ii,1);
printf("%d", received_data[ii]);
}
}
_serialPort->Close();
}
unsigned short crc_update(unsigned short crc, unsigned char data)
{
data ^= (crc & 0xff);
data ^= data << 4;
return ((((unsigned short )data << 8) | ((crc>>8)&0xff))
^ (unsigned char )(data >> 4) ^ ((unsigned short )data << 3));
}
unsigned short crc16(void *data, unsigned short cnt)
{
unsigned short crc=0xff;
unsigned char *ptr=(unsigned char *) data;
int i;
for (i=0;i<cnt;i++)
{
crc=crc_update(crc,*ptr);
ptr++;
}
return crc;
}
int main(array<System::String ^> ^args)
{
FLAGS flags;
SCIENTIFIC_COMMANDDATA scientific_commanddata;
unsigned short crc;
unsigned char *ptr = &(scientific_commanddata.packetdescriptor);
array<unsigned char> ^ startString={'>','*','>'};
unsigned short length = 0xb;
unsigned char length_high =length>>8;
unsigned char length_low =length&0xFF;
scientific_commanddata.packetdescriptor = 0x17;
scientific_commanddata.pitch= 0x04;
scientific_commanddata.roll=0x00;
scientific_commanddata.thrust=0x00;
scientific_commanddata.yaw=0x04;
scientific_commanddata.flags.pitch=0;
scientific_commanddata.flags.roll=0;
scientific_commanddata.flags.thrust=1;
scientific_commanddata.flags.yaw=1;
scientific_commanddata.flags.acc=0;
scientific_commanddata.flags.height=0;
scientific_commanddata.flags.acc_height=0;
scientific_commanddata.flags.trigger=1;
scientific_commanddata.flags.freq= 4;
unsigned char flags_low= ((scientific_commanddata.flags.pitch)+ (scientific_commanddata.flags.roll<<=1)+(scientific_commanddata.flags.thrust<<=2)+(scientific_commanddata.flags.yaw<<=3)
+(scientific_commanddata.flags.acc<<=4) + (scientific_commanddata.flags.height<<=5) +(scientific_commanddata.flags.acc_height<<=6)+(scientific_commanddata.flags.trigger<<=7) );
unsigned char flags_high = scientific_commanddata.flags.freq;
crc= crc16(ptr,length);
unsigned char crc_low = crc&0xFF;
unsigned char crc_high = crc>>8;
array<unsigned char> ^ send_data = {startString[0],
startString[1],
startString[2],
length_high,
length_low,
scientific_commanddata.packetdescriptor,
scientific_commanddata.pitch&0xFF,
scientific_commanddata.pitch>>8,
scientific_commanddata.roll&0xFF,
scientific_commanddata.roll>>8,
scientific_commanddata.thrust&0xFF,
scientific_commanddata.thrust>>8,
scientific_commanddata.yaw&0xFF,
scientific_commanddata.yaw>>8,
flags_low,
flags_high,
crc_high,
crc_low};
array<unsigned char> ^ send_data1 = {72, 0x43, 0x43, 0x00, 0x0B, 0x17, 0xF0, 0x07, 76, 0x07, 0x00, 0x00, 0xF0, 0x07, 0x8C, 0x04, 0x79, 0};
array <unsigned char> ^send_data2 = {0x3E, 0x2A, 0x3E, 0x00, 0x0B, 0x17, 0xF0, 0x07, 0xF0, 0x07, 0x00, 0x00, 0xF0, 0x07, 0x00, 0x04, 0x5C, 0x3F};
array <unsigned char> ^send_data3 = {0x3E, 0x2A, 0x3E, 0x00, 0x0B, 0x17, 0xF0, 0x07, 0xF0, 0x07, 0x00, 0x00, 0xF0, 0x07, 0x80, 0x04, 0xD0, 0xF3};
array <unsigned char> ^send_data4 = {0x3E, 0x2A, 0x3E, 0x00, 0x0B, 0x17, 0x50, 0x09, 0xF0, 0x07, 0x00, 0x00, 0xF0, 0x07, 0x00, 0x04, 0x89, 0xA9};
printf("\nsend data starstring: %d\n", sizeof(send_data));
int size =send_data->Length;
sendData_String(send_data);