aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

my quicky GCS debug library

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1886 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-16 04:52:01 +00:00
parent c07f6fb25f
commit e0834ed0f2
2 changed files with 106 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/GCS_SIMPLE/GCS_SIMPLE.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
// DIYDrones Custom ; GPS driver for ArduPilot and ArduPilotMega.
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
//
// This library is free software; you can redistribute it and / or
@ -31,6 +31,13 @@ GCS_SIMPLE::GCS_SIMPLE(Stream *s) : _port(s)
// unrecognised messages.
//
void
GCS_SIMPLE::ack(void)
{
buff_pointer = 0;
flush(1);
}
bool
GCS_SIMPLE::read(void)
{
@ -46,6 +53,8 @@ GCS_SIMPLE::read(void)
// read the next byte
data = _port->read();
//_port->write(data);
restart:
switch(_step){
case 0:
@ -64,12 +73,12 @@ GCS_SIMPLE::read(void)
goto restart;
case 2:
length = data;
_length = data;
_step++;
break;
case 3:
id = data;
_id = data;
_step++;
break;
@ -79,9 +88,66 @@ GCS_SIMPLE::read(void)
_step = 0;
parsed = true;
}
index = _buffer.msg.index;
id = _buffer.msg.id;
p1 = _buffer.msg.p1;
altitude = _buffer.msg.altitude;
latitude = _buffer.msg.latitude;
longitude = _buffer.msg.longitude;
break;
}
}
return parsed;
}
// Add binary values to the buffer
void
GCS_SIMPLE::write_byte(uint8_t val)
{
mess_buffer[buff_pointer++] = val;
}
void
GCS_SIMPLE::write_int(int val )
{
int_out.value = val;
mess_buffer[buff_pointer++] = int_out.bytes[0];
mess_buffer[buff_pointer++] = int_out.bytes[1];
}
void
GCS_SIMPLE::write_float(float val)
{
double_out.float_value = val;
mess_buffer[buff_pointer++] = double_out.bytes[0];
mess_buffer[buff_pointer++] = double_out.bytes[1];
mess_buffer[buff_pointer++] = double_out.bytes[2];
mess_buffer[buff_pointer++] = double_out.bytes[3];
}
void
GCS_SIMPLE::write_long(long val)
{
double_out.long_value = val;
mess_buffer[buff_pointer++] = double_out.bytes[0];
mess_buffer[buff_pointer++] = double_out.bytes[1];
mess_buffer[buff_pointer++] = double_out.bytes[2];
mess_buffer[buff_pointer++] = double_out.bytes[3];
}
void
GCS_SIMPLE::flush(uint8_t msg_id)
{
_port->print("4D"); // This is the message preamble
_port->write(buff_pointer); // Length
_port->write(msg_id); // id
for (uint8_t i = 0; i < buff_pointer; i++) {
_port->write(mess_buffer[i]);
}
buff_pointer = 0;
}

47
libraries/GCS_SIMPLE/GCS_SIMPLE.h
View File

@ -17,16 +17,26 @@
#include <GCS_MAVLink.h> // MAVLink GCS definitions
class GCS_SIMPLE {
public:
//GCS_SIMPLE();
GCS_SIMPLE(Stream *s);
bool read(void);
void ack(void);
void write_byte(uint8_t val);
void write_int(int val);
void write_float(float val);
void write_long(long val);
void flush(uint8_t msg_id);
Stream *_port; ///< port the GPS is attached to
private:
int8_t index;
int8_t id;
int8_t p1;
int32_t altitude;
int32_t latitude;
int32_t longitude;
#pragma pack(1)
struct diyd_mtk_msg {
int8_t index;
@ -38,17 +48,34 @@ private:
};
#pragma pack(pop)
// State machine state
uint8_t _step;
uint8_t _payload_counter;
uint8_t length;
uint8_t id;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
private:
// State machine state
// incoming
uint8_t _step;
uint8_t _payload_counter;
uint8_t _length;
uint8_t _id;
// outgoing
union d_out{
uint8_t bytes[4];
int32_t long_value;
float float_value;
} double_out;
union i_out {
uint8_t bytes[2];
int16_t value;
} int_out;
uint8_t mess_buffer[50];
uint8_t buff_pointer;
};
#endif // GCS_SIMPLE_H