first commit
This commit is contained in:
Dana Markova
85
dev/bar.cpp
Normal file
85
dev/bar.cpp
Normal file
@ -0,0 +1,85 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "i2c.h"
|
||||
|
||||
#include "bar.h"
|
||||
|
||||
static const unsigned char BAR_Addr = 0x5C; // LPS22HH
|
||||
|
||||
void (*BAR_DoneProc)(BAR_Data& Data);
|
||||
|
||||
float BAR_GetAltitude(float p0, float p1)
|
||||
{
|
||||
return 44330.0f*(1.0f-powf(p1/p0, 1.0f/5.255f));
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static inline void BAR_SetReg(unsigned char Reg, unsigned char Value)
|
||||
{
|
||||
unsigned char reg[2];
|
||||
reg[0]=Reg; reg[1]=Value;
|
||||
I2C1_Write(BAR_Addr, reg, 2);
|
||||
I2C1_Stop();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void BAR_Init()
|
||||
{
|
||||
I2C1_Init();
|
||||
|
||||
BAR_SetReg(0x10, 0x3E); // RESET
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
float BAR_GetData(float* Temp)
|
||||
{
|
||||
static float bar=0;
|
||||
static float temp=0;
|
||||
|
||||
unsigned char st;
|
||||
I2C1_Write(BAR_Addr, 0x27);
|
||||
I2C1_Read(BAR_Addr, &st, 1);
|
||||
I2C1_Stop();
|
||||
|
||||
if(st & 1)
|
||||
{
|
||||
unsigned char reg[3];
|
||||
I2C1_Write(BAR_Addr, 0x28);
|
||||
I2C1_Read(BAR_Addr, reg, sizeof(reg));
|
||||
I2C1_Stop();
|
||||
|
||||
|
||||
unsigned long b;
|
||||
|
||||
b = reg[2];
|
||||
b = (b * 256U) + reg[1];
|
||||
b = (b * 256U) + reg[0];
|
||||
b *= 256U;
|
||||
|
||||
bar=((float)b)/1048576.0f;
|
||||
}
|
||||
|
||||
if(st & 2)
|
||||
{
|
||||
unsigned char reg[2];
|
||||
I2C1_Write(BAR_Addr, 0x2B);
|
||||
I2C1_Read(BAR_Addr, reg, sizeof(reg));
|
||||
I2C1_Stop();
|
||||
|
||||
|
||||
short t;
|
||||
|
||||
t = (short)reg[1];
|
||||
t = (t * 256) + (short)reg[0];
|
||||
|
||||
temp = (float) t / 100.0f;
|
||||
}
|
||||
|
||||
*Temp=temp;
|
||||
|
||||
return bar;
|
||||
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
13
dev/bar.h
Normal file
13
dev/bar.h
Normal file
@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
struct BAR_Data
|
||||
{
|
||||
long Temp;
|
||||
float Pressure;
|
||||
};
|
||||
|
||||
void BAR_Init();
|
||||
|
||||
float BAR_GetData(float* Temp);
|
||||
void BAR_GetAsunc(void (*DoneProc)(BAR_Data& Data));
|
||||
float BAR_GetAltitude(float p0, float p1);
|
||||
97
dev/com.cpp
Normal file
97
dev/com.cpp
Normal file
@ -0,0 +1,97 @@
|
||||
#include <string.h>
|
||||
|
||||
#include "uart.h"
|
||||
#include "tick.h"
|
||||
|
||||
#include "com.h"
|
||||
|
||||
enum class MESSAGES_ID : unsigned char
|
||||
{
|
||||
SysInfo = 1,
|
||||
GyroInfo = 2,
|
||||
AccelInfo = 3,
|
||||
GpsInfo = 4,
|
||||
InertialInfo = 5,
|
||||
BatteryInfo = 6,
|
||||
Ack = 7,
|
||||
StatusCommand = 8,
|
||||
StatusAllCommands = 9,
|
||||
Command = 10,
|
||||
RequestReg = 11,
|
||||
ResponseReg = 12,
|
||||
Auth = 13,
|
||||
OpenKey = 14,
|
||||
MissionCount = 15,
|
||||
MissionItem = 16,
|
||||
MissionItemAck = 17,
|
||||
MissionRequestItem = 18,
|
||||
RequestLastMessage = 19,
|
||||
ConnectionTest = 20,
|
||||
|
||||
CountMenuCategories = 32,
|
||||
CategoriesMenu = 33,
|
||||
CategoriesParameters = 34,
|
||||
};
|
||||
|
||||
enum class COMMANDS_NAME : unsigned char
|
||||
{
|
||||
ChangeNav = 1,
|
||||
ChangeSpeed = 2,
|
||||
Land = 3,
|
||||
GoHome = 4,
|
||||
StopEngine = 5,
|
||||
StartEngine = 6,
|
||||
Pause = 7,
|
||||
Continue = 8,
|
||||
GoToGlobal = 9,
|
||||
GoToLocal = 10,
|
||||
|
||||
SetParameter = 15,
|
||||
};
|
||||
|
||||
enum class ERROR_CODE_COMMAND : unsigned char
|
||||
{
|
||||
NoError = 0,
|
||||
};
|
||||
|
||||
#pragma pack(push,1)
|
||||
|
||||
struct HeaderBegin
|
||||
{
|
||||
unsigned char stx = 0xAA;
|
||||
unsigned short len1;
|
||||
unsigned short len2;
|
||||
unsigned char crc;
|
||||
unsigned char data[0];
|
||||
|
||||
bool CheckCRC()
|
||||
{
|
||||
if(len1!=len2) return false;
|
||||
unsigned char test = 0;
|
||||
for (unsigned short a = 0; a < len1; a++) test ^= data[a];
|
||||
return crc==test;
|
||||
}
|
||||
};
|
||||
|
||||
struct HeaderMessages
|
||||
{
|
||||
MESSAGES_ID msgId;
|
||||
unsigned char srcId;
|
||||
unsigned char dstId;
|
||||
unsigned char len;
|
||||
unsigned char data[0];
|
||||
|
||||
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
void TELE_Init()
|
||||
{
|
||||
LPUART1_Init(57600);
|
||||
}
|
||||
|
||||
void TELE_Update(const void* info, unsigned long size, unsigned long update)
|
||||
{
|
||||
|
||||
}
|
||||
37
dev/crsf.cpp
Normal file
37
dev/crsf.cpp
Normal file
@ -0,0 +1,37 @@
|
||||
#include "crsf.hpp"
|
||||
#include <algorithm>
|
||||
|
||||
#include "uart.h"
|
||||
#include "tick.h"
|
||||
|
||||
uint32_t crsfLastUpdateTime;
|
||||
constexpr uint32_t crsfSignalLoseTimeout = 100;
|
||||
|
||||
void CRSF_Init(void)
|
||||
{
|
||||
UART2_Init(CRSF_BAUDRATE);
|
||||
crsfLastUpdateTime = 0;
|
||||
}
|
||||
|
||||
bool CRSF_Update(CRSF_Data& data, bool& off)
|
||||
{
|
||||
uint8_t buf[CRSF_FRAME_SIZE_MAX + 1];
|
||||
unsigned long size = UART2_Recv(buf, sizeof(buf));
|
||||
if(size == 0)
|
||||
return false;
|
||||
|
||||
uint16_t num;
|
||||
bool isUpdated = crsf_parse(buf, size, reinterpret_cast<uint16_t*>(&data), &num, 18);
|
||||
|
||||
if(isUpdated)
|
||||
crsfLastUpdateTime = TICK_GetCount();
|
||||
else if(TICK_GetCount() - crsfLastUpdateTime > crsfSignalLoseTimeout)
|
||||
{
|
||||
off = true;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
off = false;
|
||||
|
||||
return isUpdated;
|
||||
}
|
||||
31
dev/crsf.hpp
Normal file
31
dev/crsf.hpp
Normal file
@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
static constexpr uint16_t
|
||||
JOY_MIN = 172,
|
||||
JOY_MID =992,
|
||||
JOY_MAX= 1811;
|
||||
|
||||
static constexpr float JOY_VAL = (JOY_MAX - JOY_MIN) / 1e3;
|
||||
|
||||
#define CRSF_FRAME_SIZE_MAX 30 // the actual maximum length is 64, but we're only interested in RC channels and want to minimize buffer size
|
||||
#define CRSF_PAYLOAD_SIZE_MAX (CRSF_FRAME_SIZE_MAX-4)
|
||||
|
||||
#define CRSF_BAUDRATE 420000
|
||||
|
||||
struct CRSF_Data // FlySky
|
||||
{
|
||||
uint16_t X, Y, Z, W;
|
||||
uint16_t SWA, SWB, SWC, SWD;
|
||||
uint16_t VRA, VRB;
|
||||
uint16_t OTHER[8];
|
||||
|
||||
bool FrameLost;
|
||||
bool FailSafe;
|
||||
};
|
||||
|
||||
void CRSF_Init(void);
|
||||
bool CRSF_Update(CRSF_Data& data, bool& off);
|
||||
|
||||
bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
|
||||
uint16_t *num_values, uint16_t max_channels);
|
||||
293
dev/crsf_parser.cpp
Normal file
293
dev/crsf_parser.cpp
Normal file
@ -0,0 +1,293 @@
|
||||
#include "crsf.hpp"
|
||||
#include <algorithm>
|
||||
|
||||
#define CRSF_VERBOSE(...)
|
||||
#define CRSF_DEBUG(...)
|
||||
|
||||
struct crsf_frame_header_t {
|
||||
uint8_t device_address; ///< @see crsf_address_t
|
||||
uint8_t length; ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
|
||||
};
|
||||
|
||||
struct crsf_frame_t {
|
||||
crsf_frame_header_t header;
|
||||
uint8_t type; ///< @see crsf_frame_type_t
|
||||
uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
|
||||
};
|
||||
|
||||
enum class crsf_frame_type_t : uint8_t {
|
||||
gps = 0x02,
|
||||
battery_sensor = 0x08,
|
||||
link_statistics = 0x14,
|
||||
rc_channels_packed = 0x16,
|
||||
attitude = 0x1E,
|
||||
flight_mode = 0x21,
|
||||
|
||||
// Extended Header Frames, range: 0x28 to 0x96
|
||||
device_ping = 0x28,
|
||||
device_info = 0x29,
|
||||
parameter_settings_entry = 0x2B,
|
||||
parameter_read = 0x2C,
|
||||
parameter_write = 0x2D,
|
||||
command = 0x32
|
||||
};
|
||||
|
||||
enum class crsf_payload_size_t : uint8_t {
|
||||
gps = 15,
|
||||
battery_sensor = 8,
|
||||
link_statistics = 10,
|
||||
rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
|
||||
attitude = 6,
|
||||
};
|
||||
|
||||
enum class crsf_parser_state_t : uint8_t {
|
||||
unsynced = 0,
|
||||
synced
|
||||
};
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct crsf_payload_RC_channels_packed_t {
|
||||
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
|
||||
unsigned chan0 : 11;
|
||||
unsigned chan1 : 11;
|
||||
unsigned chan2 : 11;
|
||||
unsigned chan3 : 11;
|
||||
unsigned chan4 : 11;
|
||||
unsigned chan5 : 11;
|
||||
unsigned chan6 : 11;
|
||||
unsigned chan7 : 11;
|
||||
unsigned chan8 : 11;
|
||||
unsigned chan9 : 11;
|
||||
unsigned chan10 : 11;
|
||||
unsigned chan11 : 11;
|
||||
unsigned chan12 : 11;
|
||||
unsigned chan13 : 11;
|
||||
unsigned chan14 : 11;
|
||||
unsigned chan15 : 11;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
static crsf_frame_t crsf_frame;
|
||||
static unsigned current_frame_position = 0;
|
||||
static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
|
||||
|
||||
/**
|
||||
* parse the current crsf_frame buffer
|
||||
*/
|
||||
bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
|
||||
uint16_t *num_values, uint16_t max_channels);
|
||||
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
|
||||
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
|
||||
|
||||
bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
|
||||
uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
bool ret = false;
|
||||
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
|
||||
|
||||
while (len > 0) {
|
||||
|
||||
// fill in the crsf_buffer, as much as we can
|
||||
const unsigned current_len = std::min(len, sizeof(crsf_frame_t) - current_frame_position);
|
||||
memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
|
||||
current_frame_position += current_len;
|
||||
|
||||
// protection to guarantee parsing progress
|
||||
if (current_len == 0) {
|
||||
CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
|
||||
|
||||
for (unsigned i = 0; i < current_frame_position; ++i) {
|
||||
CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
|
||||
}
|
||||
|
||||
// reset the parser
|
||||
current_frame_position = 0;
|
||||
parser_state = crsf_parser_state_t::unsynced;
|
||||
return false;
|
||||
}
|
||||
|
||||
len -= current_len;
|
||||
frame += current_len;
|
||||
|
||||
if (crsf_parse_buffer(values, num_values, max_channels)) {
|
||||
ret = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
|
||||
|
||||
if (parser_state == crsf_parser_state_t::unsynced) {
|
||||
// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
|
||||
for (unsigned i = 1; i < current_frame_position - 1; ++i) {
|
||||
if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
|
||||
crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
|
||||
parser_state = crsf_parser_state_t::synced;
|
||||
unsigned frame_offset = i - 1;
|
||||
CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
|
||||
|
||||
// move the rest of the buffer to the beginning
|
||||
if (frame_offset != 0) {
|
||||
memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
|
||||
current_frame_position -= frame_offset;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (parser_state != crsf_parser_state_t::synced) {
|
||||
if (current_frame_position >= sizeof(crsf_frame_t)) {
|
||||
// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
|
||||
current_frame_position = 3;
|
||||
|
||||
for (unsigned i = 0; i < current_frame_position; ++i) {
|
||||
crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
|
||||
}
|
||||
|
||||
CRSF_VERBOSE("Discarding buffer");
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
if (current_frame_position < 3) {
|
||||
// wait until we have the header & type
|
||||
return false;
|
||||
}
|
||||
|
||||
// Now we have at least the header and the type
|
||||
|
||||
const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
|
||||
|
||||
if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
|
||||
// frame too long or bogus -> discard everything and go into unsynced state
|
||||
current_frame_position = 0;
|
||||
parser_state = crsf_parser_state_t::unsynced;
|
||||
CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (current_frame_position < current_frame_length) {
|
||||
// we don't have the full frame yet -> wait for more data
|
||||
CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = false;
|
||||
|
||||
// Now we have the full frame
|
||||
|
||||
if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
|
||||
crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
|
||||
const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
|
||||
|
||||
if (crc == crsf_frame_CRC(crsf_frame)) {
|
||||
const crsf_payload_RC_channels_packed_t *const rc_channels =
|
||||
(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
|
||||
*num_values = std::min<uint16_t>(max_channels, 16);
|
||||
|
||||
if (max_channels > 0) { values[0] = (rc_channels->chan0); }
|
||||
|
||||
if (max_channels > 1) { values[1] = (rc_channels->chan1); }
|
||||
|
||||
if (max_channels > 2) { values[2] = (rc_channels->chan2); }
|
||||
|
||||
if (max_channels > 3) { values[3] = (rc_channels->chan3); }
|
||||
|
||||
if (max_channels > 4) { values[4] = (rc_channels->chan4); }
|
||||
|
||||
if (max_channels > 5) { values[5] = (rc_channels->chan5); }
|
||||
|
||||
if (max_channels > 6) { values[6] = (rc_channels->chan6); }
|
||||
|
||||
if (max_channels > 7) { values[7] = (rc_channels->chan7); }
|
||||
|
||||
if (max_channels > 8) { values[8] = (rc_channels->chan8); }
|
||||
|
||||
if (max_channels > 9) { values[9] = (rc_channels->chan9); }
|
||||
|
||||
if (max_channels > 10) { values[10] = (rc_channels->chan10); }
|
||||
|
||||
if (max_channels > 11) { values[11] = (rc_channels->chan11); }
|
||||
|
||||
if (max_channels > 12) { values[12] = (rc_channels->chan12); }
|
||||
|
||||
if (max_channels > 13) { values[13] = (rc_channels->chan13); }
|
||||
|
||||
if (max_channels > 14) { values[14] = (rc_channels->chan14); }
|
||||
|
||||
if (max_channels > 15) { values[15] = (rc_channels->chan15); }
|
||||
|
||||
CRSF_VERBOSE("Got Channels");
|
||||
|
||||
ret = true;
|
||||
|
||||
} else {
|
||||
CRSF_DEBUG("CRC check failed");
|
||||
}
|
||||
|
||||
} else {
|
||||
CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
|
||||
// We could check the CRC here and reset the parser into unsynced state if it fails.
|
||||
// But in practise it's robust even without that.
|
||||
}
|
||||
|
||||
// Either reset or move the rest of the buffer
|
||||
if (current_frame_position > current_frame_length) {
|
||||
CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
|
||||
memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
|
||||
current_frame_position -= current_frame_length;
|
||||
|
||||
} else {
|
||||
current_frame_position = 0;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
|
||||
{
|
||||
crc ^= a;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0xD5;
|
||||
|
||||
} else {
|
||||
crc = crc << 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = crc8_dvb_s2(crc, buf[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
|
||||
{
|
||||
// CRC includes type and payload
|
||||
uint8_t crc = crc8_dvb_s2(0, frame.type);
|
||||
|
||||
for (int i = 0; i < frame.header.length - 2; ++i) {
|
||||
crc = crc8_dvb_s2(crc, frame.payload[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
35
dev/eep.cpp
Normal file
35
dev/eep.cpp
Normal file
@ -0,0 +1,35 @@
|
||||
#include "i2c.h"
|
||||
|
||||
#include "eep.h"
|
||||
|
||||
static const unsigned char EEP_Addr = 0x50; // AT24C256
|
||||
|
||||
static inline short Rev16(short v)
|
||||
{
|
||||
asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
|
||||
return v;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void EEP_Init() // AT24C256
|
||||
{
|
||||
I2C1_Init();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void EEP_Read(unsigned short Addr, void* Data, unsigned short Size)
|
||||
{
|
||||
Addr=Rev16(Addr);
|
||||
I2C1_Write(EEP_Addr, &Addr, 2);
|
||||
I2C1_Read(EEP_Addr, Data, Size);
|
||||
I2C1_Stop();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void EEP_Write(unsigned short Addr, const void* Data, unsigned short Size)
|
||||
{
|
||||
Addr=Rev16(Addr);
|
||||
I2C1_Write2(EEP_Addr, &Addr, 2, Data, Size);
|
||||
I2C1_Stop();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
6
dev/eep.h
Normal file
6
dev/eep.h
Normal file
@ -0,0 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
void EEP_Init();
|
||||
|
||||
void EEP_Read(unsigned short Addr, void* Data, unsigned short Size);
|
||||
void EEP_Write(unsigned short Addr, const void* Data, unsigned short Size);
|
||||
130
dev/flow.cpp
Normal file
130
dev/flow.cpp
Normal file
@ -0,0 +1,130 @@
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "gpio.h"
|
||||
|
||||
#include "spi.h"
|
||||
#include "tick.h"
|
||||
|
||||
#include "tof.h"
|
||||
|
||||
static void WriteReg(char reg, char value)
|
||||
{
|
||||
char send[2]={reg | 0x80, value};
|
||||
SPI1_TransferCons(send, 2, 0, 0);
|
||||
}
|
||||
|
||||
bool FLOW_Init() // PMW3901
|
||||
{
|
||||
SPI1_Init();
|
||||
|
||||
WriteReg(0x3A, 0x5A);
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
|
||||
// Test the SPI communication, checking chipId and inverse chipId
|
||||
|
||||
char reg[4]={0x00, 0, 0x5F, 0};
|
||||
SPI1_TransferParallel(reg, reg, 4);
|
||||
|
||||
if (reg[1] != 0x49 || reg[3] != 0xB6) return false;
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
|
||||
WriteReg(0x7F, 0x00);
|
||||
WriteReg(0x61, 0xAD);
|
||||
WriteReg(0x7F, 0x03);
|
||||
WriteReg(0x40, 0x00);
|
||||
WriteReg(0x7F, 0x05);
|
||||
WriteReg(0x41, 0xB3);
|
||||
WriteReg(0x43, 0xF1);
|
||||
WriteReg(0x45, 0x14);
|
||||
WriteReg(0x5B, 0x32);
|
||||
WriteReg(0x5F, 0x34);
|
||||
WriteReg(0x7B, 0x08);
|
||||
WriteReg(0x7F, 0x06);
|
||||
WriteReg(0x44, 0x1B);
|
||||
WriteReg(0x40, 0xBF);
|
||||
WriteReg(0x4E, 0x3F);
|
||||
|
||||
WriteReg(0x7F, 0x08);
|
||||
WriteReg(0x65, 0x20);
|
||||
WriteReg(0x6A, 0x18);
|
||||
WriteReg(0x7F, 0x09);
|
||||
WriteReg(0x4F, 0xAF);
|
||||
WriteReg(0x5F, 0x40);
|
||||
WriteReg(0x48, 0x80);
|
||||
WriteReg(0x49, 0x80);
|
||||
WriteReg(0x57, 0x77);
|
||||
WriteReg(0x60, 0x78);
|
||||
WriteReg(0x61, 0x78);
|
||||
WriteReg(0x62, 0x08);
|
||||
WriteReg(0x63, 0x50);
|
||||
WriteReg(0x7F, 0x0A);
|
||||
WriteReg(0x45, 0x60);
|
||||
WriteReg(0x7F, 0x00);
|
||||
WriteReg(0x4D, 0x11);
|
||||
WriteReg(0x55, 0x80);
|
||||
WriteReg(0x74, 0x1F);
|
||||
WriteReg(0x75, 0x1F);
|
||||
WriteReg(0x4A, 0x78);
|
||||
WriteReg(0x4B, 0x78);
|
||||
WriteReg(0x44, 0x08);
|
||||
WriteReg(0x45, 0x50);
|
||||
WriteReg(0x64, 0xFF);
|
||||
WriteReg(0x65, 0x1F);
|
||||
WriteReg(0x7F, 0x14);
|
||||
WriteReg(0x65, 0x60);
|
||||
WriteReg(0x66, 0x08);
|
||||
WriteReg(0x63, 0x78);
|
||||
WriteReg(0x7F, 0x15);
|
||||
WriteReg(0x48, 0x58);
|
||||
WriteReg(0x7F, 0x07);
|
||||
WriteReg(0x41, 0x0D);
|
||||
WriteReg(0x43, 0x14);
|
||||
WriteReg(0x4B, 0x0E);
|
||||
WriteReg(0x45, 0x0F);
|
||||
WriteReg(0x44, 0x42);
|
||||
WriteReg(0x4C, 0x80);
|
||||
WriteReg(0x7F, 0x10);
|
||||
WriteReg(0x5B, 0x02);
|
||||
WriteReg(0x7F, 0x07);
|
||||
WriteReg(0x40, 0x41);
|
||||
WriteReg(0x70, 0x00);
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
|
||||
WriteReg(0x32, 0x44);
|
||||
WriteReg(0x7F, 0x07);
|
||||
WriteReg(0x40, 0x40);
|
||||
WriteReg(0x7F, 0x06);
|
||||
WriteReg(0x62, 0xf0);
|
||||
WriteReg(0x63, 0x00);
|
||||
WriteReg(0x7F, 0x0D);
|
||||
WriteReg(0x48, 0xC0);
|
||||
WriteReg(0x6F, 0xd5);
|
||||
WriteReg(0x7F, 0x00);
|
||||
WriteReg(0x5B, 0xa0);
|
||||
WriteReg(0x4E, 0xA8);
|
||||
WriteReg(0x5A, 0x50);
|
||||
WriteReg(0x40, 0x80);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FLOW_GetMotion(short* dX, short* dY, unsigned char* qual)
|
||||
{
|
||||
char reg[12]={0x02, 0, 0x03, 0, 0x04, 0, 0x05, 0, 0x06, 0, 0x07, 0};
|
||||
|
||||
SPI1_TransferParallel(reg, reg, 12);
|
||||
|
||||
short x = ((short)reg[5] << 8) | reg[3];
|
||||
short y = ((short)reg[9] << 8) | reg[7];
|
||||
|
||||
if(dX) *dX = x;
|
||||
if(dY) *dY = y;
|
||||
|
||||
if(qual) *qual = reg[11];
|
||||
|
||||
return reg[1] & 0x80;
|
||||
}
|
||||
4
dev/flow.h
Normal file
4
dev/flow.h
Normal file
@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
bool FLOW_Init();
|
||||
bool FLOW_GetMotion(short* dX, short* dY, unsigned char* qual);
|
||||
588
dev/gps.cpp
Normal file
588
dev/gps.cpp
Normal file
@ -0,0 +1,588 @@
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "gpio.h"
|
||||
|
||||
#include "uart.h"
|
||||
#include "tick.h"
|
||||
#include "ori.h"
|
||||
|
||||
#include "filt.h"
|
||||
|
||||
#include "gps.h"
|
||||
|
||||
//FilterGPS filt_x, filt_y, filt_z;
|
||||
|
||||
/*void GPS_Navigation(bool valid, ORI_Data& data, float p[3])
|
||||
{
|
||||
filt_x.Update(valid, p[0], data.Iner.X/1000);
|
||||
filt_y.Update(valid, p[1], data.Iner.Y/1000);
|
||||
filt_z.Update(valid, p[2], data.Iner.Z/1000);
|
||||
|
||||
data.Speed.X=filt_x.Speed.Value;
|
||||
data.Speed.Y=filt_y.Speed.Value;
|
||||
data.Speed.Z=filt_z.Speed.Value;
|
||||
|
||||
data.Pos.X=filt_x.Position.Value;
|
||||
data.Pos.Y=filt_y.Position.Value;
|
||||
data.Pos.Z=filt_z.Position.Value;
|
||||
}*/
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
Point Reverse_GPS_LocalDistance(Point p0, float dx, float dy) // light formula
|
||||
{
|
||||
const float pi = 3.14159265359f;
|
||||
const float er = 6371000.0f; // Radius of the earth in m
|
||||
float D = pi/180.0f;
|
||||
|
||||
Point p1;
|
||||
|
||||
p1.Latitude = (dy/(er*D))+p0.Latitude;
|
||||
|
||||
float l = ((float)(p0.Latitude + p1.Latitude)) / 2.0f;
|
||||
float r = cosf(l*pi/180.0f) * er;
|
||||
p1.Longitude = dx/(r*D)+p0.Longitude;
|
||||
|
||||
return p1;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
float GPS_LocalDistance(Point p1, Point p2, float& dx, float& dy) // light formula
|
||||
{
|
||||
const float pi = 3.14159265359f;
|
||||
const float er = 6371000.0f; // Radius of the earth in m
|
||||
|
||||
float lat = p1.Latitude - p2.Latitude;
|
||||
float lon = p1.Longitude - p2.Longitude;
|
||||
|
||||
float y = er/360.0f * (lat*pi*2.0f); // lat
|
||||
dy = y;
|
||||
|
||||
float l = ((float)(p1.Latitude + p2.Latitude)) / 2.0f;
|
||||
float r = cosf(l*pi/180.0f) * er;
|
||||
float x = r/360.0f * (lon*pi*2.0f); // long
|
||||
dx = x;
|
||||
|
||||
float d = sqrtf(x*x + y*y);
|
||||
return d;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
float GPS_GlobalDistance(Point p1, Point p2) // Haversine formula
|
||||
{
|
||||
const float pi = 3.14159265359f;
|
||||
const float er = 6371000.0f; // Radius of the earth in m
|
||||
|
||||
float dLat = ((float)(p2.Latitude - p1.Latitude))*pi/180.0f;
|
||||
float dLon = ((float)(p2.Longitude - p1.Longitude))*pi/180.0f;
|
||||
|
||||
float lat1 = p1.Latitude, lat2 = p2.Latitude;
|
||||
|
||||
float sdlat=sinf(dLat/2.0f);
|
||||
float sdlon=sinf(dLon/2.0f);
|
||||
|
||||
float a = (sdlat*sdlat) + cosf((lat1)*pi/180.0f) * cosf((lat2)*pi/180.0f) * (sdlon*sdlon);
|
||||
float c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f-a));
|
||||
float d = er * c; // Distance in m
|
||||
return d;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
struct GPS_Head
|
||||
{
|
||||
static const unsigned long Size = 7;
|
||||
const char* NMEA;
|
||||
};
|
||||
|
||||
struct GPS_GNRMC // $GNRMC,hhmmss.sss,A,ggmm.mm,P,gggmm.mm,J,v.v,b.b,ddmmyy,x.x,n,m*hh/r/n
|
||||
{
|
||||
static constexpr GPS_Head Head = {"$GNRMC,"}; // Recommended minimum specific Transit data
|
||||
//---
|
||||
char* Time; // UTC of position fix
|
||||
char* Valid; // Data status (V=navigation receiver warning)
|
||||
char* Latitude; // Latitude of fix
|
||||
char* Pole; // N or S
|
||||
char* Longitude; // Longitude of fix
|
||||
char* J; // E or W
|
||||
char* HorSpeed; // Speed over ground in knots
|
||||
char* Angle; // Track made good in degrees True
|
||||
char* Date; // UT date
|
||||
char* Mag; // Magnetic variation degrees (Easterly var. subtracts from true course)
|
||||
char* MagAng; // Magnetic declination direction, E (east) or W (west)
|
||||
char* Mode; // Mode indication (A=autonomous positioning, D=differential, E=estimation, N=invalid data)
|
||||
//---
|
||||
// Checksum
|
||||
};
|
||||
|
||||
struct GPS_GNGGA // $GNGGA,hhmmss.ss,ggmm.mm,a,gggmm.mm,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh/r/n
|
||||
{
|
||||
static constexpr GPS_Head Head = {"$GNGGA,"}; // Global Positioning System Fix Data
|
||||
//---
|
||||
char* Time; // UTC of position
|
||||
char* Latitude; // latitude of position
|
||||
char* Pole; // N or S
|
||||
char* Longitude; // Longitude of position
|
||||
char* J; // E or W
|
||||
char* Quality; // GPS Quality indicator (0=no fix, 1=GPS fix, 2=Dif. GPS fix)
|
||||
char* Satellites; // Number of satellites in use [not those in view]
|
||||
char* HDOP; // Horizontal dilution of position
|
||||
char* Altitude; // Antenna altitude above/below mean sea level (geoid)
|
||||
char* UnitsAlt; // Meters (Antenna height unit)
|
||||
char* Geoidal; // Geoidal separation (Diff. between WGS-84 earth ellipsoid andmean sea level. -=geoid is below WGS-84 ellipsoid)
|
||||
char* UnitsGeoidal; // Meters (Units of geoidal separation)
|
||||
char* LastUpdate; // Age in seconds since last update from diff. reference station
|
||||
char* StationID; // Diff. reference station ID#
|
||||
//---
|
||||
// Checksum
|
||||
};
|
||||
|
||||
struct GPS_GNGSA // $GNGSA,mode,fix,PRN(11),PDOP,HDOP,VDOP,*hh/r/n
|
||||
{
|
||||
static constexpr GPS_Head Head = {"$GNGSA,"}; // Global Positioning System Fix Data
|
||||
//---
|
||||
char* Mode; // M = Manual, A = Automatic
|
||||
char* FixType; // 1 = not available, 2 = 2D, 3 = 3D
|
||||
char* ID_1; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
|
||||
char* ID_2;
|
||||
char* ID_3;
|
||||
char* ID_4;
|
||||
char* ID_5;
|
||||
char* ID_6;
|
||||
char* ID_7;
|
||||
char* ID_8;
|
||||
char* ID_9;
|
||||
char* ID_10;
|
||||
char* ID_11;
|
||||
char* ID_12;
|
||||
char* PDOP;
|
||||
char* HDOP;
|
||||
char* VDOP;
|
||||
//---
|
||||
// Checksum
|
||||
};
|
||||
|
||||
struct GPS_GPGSV // $GPGSV,count,index,visible,[ID,elevation,azimuth,SNR]*hh/r/n
|
||||
{
|
||||
static constexpr GPS_Head Head = {"$GPGSV,"}; // Global Positioning System Fix Data
|
||||
//---
|
||||
char* Count; // M = Manual, A = Automatic
|
||||
char* Index; // 1 = not available, 2 = 2D, 3 = 3D
|
||||
char* Visible; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
|
||||
|
||||
char* ID_1;
|
||||
char* Elevation_1;
|
||||
char* Azimuth_1;
|
||||
char* SNR_1;
|
||||
|
||||
char* ID_2;
|
||||
char* Elevation_2;
|
||||
char* Azimuth_2;
|
||||
char* SNR_2;
|
||||
|
||||
char* ID_3;
|
||||
char* Elevation_3;
|
||||
char* Azimuth_3;
|
||||
char* SNR_3;
|
||||
|
||||
char* ID_4;
|
||||
char* Elevation_4;
|
||||
char* Azimuth_4;
|
||||
char* SNR_4;
|
||||
|
||||
//---
|
||||
// Checksum
|
||||
};
|
||||
|
||||
struct GPS_GLGSV // $GLGSV,count,index,visible,[ID,elevation,azimuth,SNR]*hh/r/n
|
||||
{
|
||||
static constexpr GPS_Head Head = {"$GLGSV,"}; // Global Positioning System Fix Data
|
||||
//---
|
||||
char* Count; // M = Manual, A = Automatic
|
||||
char* Index; // 1 = not available, 2 = 2D, 3 = 3D
|
||||
char* Visible; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
|
||||
|
||||
char* ID_1;
|
||||
char* Elevation_1;
|
||||
char* Azimuth_1;
|
||||
char* SNR_1;
|
||||
|
||||
char* ID_2;
|
||||
char* Elevation_2;
|
||||
char* Azimuth_2;
|
||||
char* SNR_2;
|
||||
|
||||
char* ID_3;
|
||||
char* Elevation_3;
|
||||
char* Azimuth_3;
|
||||
char* SNR_3;
|
||||
|
||||
char* ID_4;
|
||||
char* Elevation_4;
|
||||
char* Azimuth_4;
|
||||
char* SNR_4;
|
||||
|
||||
//---
|
||||
// Checksum
|
||||
};
|
||||
|
||||
enum class GPS_PROTO {GNRMC, GNGGA, GNGSA, GPGSV, GLGSV, COUNT};
|
||||
static const long GPS_ProtCount = (long)GPS_PROTO::COUNT;
|
||||
static GPS_Head GPS_Protocols[GPS_ProtCount] = { GPS_GNRMC::Head, GPS_GNGGA::Head, GPS_GNGSA::Head, GPS_GPGSV::Head, GPS_GLGSV::Head };
|
||||
|
||||
struct GPS_Info
|
||||
{
|
||||
GPS_PROTO Type;
|
||||
|
||||
static const unsigned long Length = 256;
|
||||
char Data[Length];
|
||||
|
||||
static const unsigned long Count = 32;
|
||||
char* Param[Count];
|
||||
|
||||
long Begin = 0;
|
||||
long Index = 0;
|
||||
long Size = 0;
|
||||
|
||||
unsigned char CRC8;
|
||||
};
|
||||
|
||||
|
||||
static GPS_Info Info;
|
||||
|
||||
void (*GPS_CallBack)(GPS_PROTO Proto, void* Data)=0;
|
||||
|
||||
//------------------------------------------------------------------------------------------------------------------------------
|
||||
/*static unsigned long UARTt_Recv(void* Data, unsigned long Size, bool WaitAll=false) // For Check ONLY !!!!!!!!!!!!!!!!
|
||||
{
|
||||
static int begin=0;
|
||||
|
||||
const char test[]="$GNGGA,122013.10,4719.45110,N,03846.54105,E,1,12,0.66,35.9,M,17.6,M,,*7A\r\n";
|
||||
unsigned long size=strlen(test);
|
||||
|
||||
memcpy(Data, test, Size<size ? Size : size);
|
||||
//memcpy(Data, test+begin, 1);
|
||||
|
||||
begin++;
|
||||
|
||||
if(begin>=size) begin=0;
|
||||
|
||||
//return 1;
|
||||
return size;
|
||||
}*/
|
||||
//------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
void GPS_Update()
|
||||
{
|
||||
char* head = Info.Data;
|
||||
|
||||
Info.Size += UART1_Recv(head + Info.Size, Info.Length - Info.Size);
|
||||
|
||||
long size = Info.Size - Info.Index;
|
||||
|
||||
while (size)
|
||||
{
|
||||
char* data = head + Info.Index;
|
||||
|
||||
if (Info.Index && *data == '$')
|
||||
{
|
||||
memcpy(head, data, size);
|
||||
Info.Index = 0;
|
||||
Info.Begin = 0;
|
||||
Info.Size = size;
|
||||
data = head;
|
||||
}
|
||||
|
||||
if (!Info.Begin)
|
||||
{
|
||||
if (*head != '$')
|
||||
{
|
||||
Info.Index++;
|
||||
size--;
|
||||
continue;
|
||||
}
|
||||
//---
|
||||
if (Info.Size < GPS_Head::Size) return;
|
||||
//---
|
||||
Info.Type = GPS_PROTO::COUNT;
|
||||
for (long a = 0; a < GPS_ProtCount; a++)
|
||||
{
|
||||
const char* nmea = GPS_Protocols[a].NMEA;
|
||||
if (memcmp(head, nmea, GPS_Head::Size) == 0)
|
||||
{
|
||||
Info.CRC8 = 0x0E;
|
||||
for (long a = 0; nmea[a]; a++) Info.CRC8 ^= nmea[a];
|
||||
Info.Type = (GPS_PROTO)a;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//---
|
||||
if (Info.Type == GPS_PROTO::COUNT)
|
||||
{
|
||||
Info.Index++;
|
||||
*head = '#';
|
||||
size--;
|
||||
continue;
|
||||
}
|
||||
//---
|
||||
Info.Index = GPS_Head::Size;
|
||||
size = Info.Size - GPS_Head::Size;
|
||||
//---
|
||||
Info.Param[Info.Begin++] = head + Info.Index;
|
||||
//---
|
||||
continue;
|
||||
}
|
||||
//---
|
||||
if (Info.Size > Info.Length)
|
||||
{
|
||||
Info.Begin = 0;
|
||||
Info.Index = 0;
|
||||
Info.Size = 0;
|
||||
return;
|
||||
}
|
||||
//---
|
||||
if (Info.Begin >= Info.Count)
|
||||
{
|
||||
if (*data == '\n')
|
||||
{
|
||||
*data = '\0';
|
||||
size--;
|
||||
char* end;
|
||||
unsigned char crc8 = strtol(data-3, &end, 16);
|
||||
if ((end == data - 1) && (crc8 == Info.CRC8))
|
||||
{
|
||||
if (GPS_CallBack) GPS_CallBack(Info.Type, Info.Param);
|
||||
//---
|
||||
if (size) memcpy(Info.Data, data + 1, size);
|
||||
}
|
||||
//---
|
||||
Info.Begin = 0;
|
||||
Info.Index = 0;
|
||||
Info.Size = size;
|
||||
//---
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Info.CRC8 ^= *data;
|
||||
//---
|
||||
if (*data == ',')
|
||||
{
|
||||
*data = '\0';
|
||||
Info.Param[Info.Begin++] = ++data;
|
||||
}
|
||||
else if (*data == '*')
|
||||
{
|
||||
*data++ = '\0';
|
||||
while (Info.Begin < Info.Count)
|
||||
{
|
||||
Info.Param[Info.Begin++] = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
//---
|
||||
Info.Index++;
|
||||
size--;
|
||||
}
|
||||
|
||||
if (!Info.Begin && Info.Index)
|
||||
{
|
||||
Info.Index = 0;
|
||||
Info.Size = 0;
|
||||
}
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static unsigned char GPS_VisibleGPS=0;
|
||||
static unsigned char GPS_VisibleGLO=0;
|
||||
|
||||
static unsigned char Base_Noise[256];
|
||||
|
||||
Point GetCoordinates_Coord;
|
||||
bool GetCoordinates_Valid;
|
||||
bool GetCoordinates_Update;
|
||||
|
||||
Point Base_BeginXYZ;
|
||||
|
||||
static float Base_Alt=0, Base_Geo=0;
|
||||
static unsigned char Base_Used, Base_Fix;
|
||||
|
||||
static GPS_BaseInfo GPS_BaseInfoData;
|
||||
|
||||
static void GPS_CallBack_GSV(GPS_PROTO Proto, void* Data)
|
||||
{
|
||||
if(Proto==GPS_PROTO::GNGGA)
|
||||
{
|
||||
GetCoordinates_Update=true;
|
||||
|
||||
GPS_GNGGA* data=(GPS_GNGGA*)Data;
|
||||
|
||||
GetCoordinates_Valid = (data->Quality[0] && (data->Quality[0]=='1' || data->Quality[0]=='2'));
|
||||
|
||||
float geo=0;
|
||||
if(data->Geoidal[0] && GetCoordinates_Valid) geo=strtof(data->Geoidal, 0);
|
||||
|
||||
if(data->Altitude[0]) GetCoordinates_Coord.Altitude=strtof(data->Altitude, 0);
|
||||
//---
|
||||
Base_Geo=GetCoordinates_Coord.Altitude+geo;
|
||||
|
||||
|
||||
Base_Alt=Base_Geo-Base_BeginXYZ.Altitude;
|
||||
//---
|
||||
GetCoordinates_Coord.Altitude+=geo;
|
||||
|
||||
if(data->Latitude[0] && GetCoordinates_Valid)
|
||||
{
|
||||
GetCoordinates_Coord.Latitude=(data->Latitude[0]-'0')*10 + data->Latitude[1]-'0';
|
||||
GetCoordinates_Coord.Latitude+=strtod(data->Latitude+2, 0)/60;
|
||||
}
|
||||
//---
|
||||
if(data->Longitude[0] && GetCoordinates_Valid)
|
||||
{
|
||||
GetCoordinates_Coord.Longitude=(data->Longitude[0]-'0')*100 + (data->Longitude[1]-'0')*10 + data->Longitude[2]-'0';
|
||||
GetCoordinates_Coord.Longitude+=strtod(data->Longitude+3, 0)/60;
|
||||
}
|
||||
|
||||
if(data->Satellites[0] && GetCoordinates_Valid) Base_Used=strtoul(data->Satellites, 0, 10);
|
||||
|
||||
if(data->Quality[0] && GetCoordinates_Valid) Base_Fix=strtoul(data->Quality, 0, 10);
|
||||
}
|
||||
|
||||
if(Proto==GPS_PROTO::GPGSV)
|
||||
{
|
||||
GPS_GPGSV* data=(GPS_GPGSV*)Data;
|
||||
GPS_VisibleGPS = strtoul(data->Visible, 0, 10);
|
||||
|
||||
if(!data->Index || *data->Index=='1') memset(Base_Noise, 0, sizeof(Base_Noise));
|
||||
|
||||
long id;
|
||||
|
||||
if(data->ID_1)
|
||||
{
|
||||
id=strtoul(data->ID_1, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_1, 0, 10);
|
||||
}
|
||||
if(data->ID_2)
|
||||
{
|
||||
id=strtoul(data->ID_2, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_2, 0, 10);
|
||||
}
|
||||
if(data->ID_3)
|
||||
{
|
||||
id=strtoul(data->ID_3, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_3, 0, 10);
|
||||
}
|
||||
if(data->ID_4)
|
||||
{
|
||||
id=strtoul(data->ID_4, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_4, 0, 10);
|
||||
}
|
||||
}
|
||||
else if(Proto==GPS_PROTO::GLGSV)
|
||||
{
|
||||
GPS_GLGSV* data=(GPS_GLGSV*)Data;
|
||||
GPS_VisibleGLO = strtoul(data->Visible, 0, 10);
|
||||
|
||||
long id;
|
||||
|
||||
if(data->ID_1)
|
||||
{
|
||||
id=strtoul(data->ID_1, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_1, 0, 10);
|
||||
}
|
||||
if(data->ID_2)
|
||||
{
|
||||
id=strtoul(data->ID_2, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_2, 0, 10);
|
||||
}
|
||||
if(data->ID_3)
|
||||
{
|
||||
id=strtoul(data->ID_3, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_3, 0, 10);
|
||||
}
|
||||
if(data->ID_4)
|
||||
{
|
||||
id=strtoul(data->ID_4, 0, 10);
|
||||
if(id<256) Base_Noise[id]=strtoul(data->SNR_4, 0, 10);
|
||||
}
|
||||
|
||||
if(*data->Count==*data->Index)
|
||||
{
|
||||
float noise=0, count=0;
|
||||
for(long a=0; a<256; a++)
|
||||
{
|
||||
if(!Base_Noise[a]) continue;
|
||||
noise+=Base_Noise[a];
|
||||
count++;
|
||||
}
|
||||
|
||||
if(count) GPS_BaseInfoData.noise=noise/count;
|
||||
else GPS_BaseInfoData.noise=0;
|
||||
|
||||
GPS_BaseInfoData.satVisible=GPS_VisibleGPS+GPS_VisibleGLO;
|
||||
}
|
||||
}
|
||||
else if(Proto==GPS_PROTO::GNRMC)
|
||||
{
|
||||
GPS_GNRMC* data=(GPS_GNRMC*)Data;
|
||||
|
||||
GPS_BaseInfoData.lat=GetCoordinates_Coord.Latitude;
|
||||
GPS_BaseInfoData.lon=GetCoordinates_Coord.Longitude;
|
||||
|
||||
if(data->HorSpeed[0]) GPS_BaseInfoData.speed=strtof(data->HorSpeed, 0);
|
||||
|
||||
if(data->Time[0]) GPS_BaseInfoData.timeUTC=strtof(data->Time, 0);
|
||||
|
||||
float dx, dy;
|
||||
GPS_LocalDistance(Base_BeginXYZ, {GPS_BaseInfoData.lat, GPS_BaseInfoData.lon}, dx, dy);
|
||||
|
||||
GPS_BaseInfoData.LocalXYZ[0]=dx;
|
||||
GPS_BaseInfoData.LocalXYZ[1]=-dy;
|
||||
GPS_BaseInfoData.LocalXYZ[2]=Base_Geo-Base_BeginXYZ.Altitude;
|
||||
}
|
||||
else if(Proto==GPS_PROTO::GNGSA)
|
||||
{
|
||||
GPS_GNGSA* data=(GPS_GNGSA*)Data;
|
||||
|
||||
if(data->HDOP[0]) GPS_BaseInfoData.hdop=strtof(data->HDOP, 0);
|
||||
if(data->VDOP[0]) GPS_BaseInfoData.vdop=strtof(data->VDOP, 0);
|
||||
if(data->PDOP[0]) GPS_BaseInfoData.pdop=strtof(data->PDOP, 0);
|
||||
}
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void GPS_Init()
|
||||
{
|
||||
UART1_Init(230400);
|
||||
GPS_CallBack=GPS_CallBack_GSV;
|
||||
memset(Base_Noise, 0, sizeof(Base_Noise));
|
||||
}
|
||||
|
||||
bool GPS_GetCoordinates(Point& Coord, bool& Valid)
|
||||
{
|
||||
if(!GetCoordinates_Update) return false;
|
||||
|
||||
GetCoordinates_Update=false;
|
||||
|
||||
Coord=GetCoordinates_Coord;
|
||||
Valid=GetCoordinates_Valid;
|
||||
|
||||
return true;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
bool GPS_GetBaseInfo(GPS_BaseInfo& Info)
|
||||
{
|
||||
Info=GPS_BaseInfoData;
|
||||
|
||||
Info.realAlt=Base_Alt;
|
||||
Info.absAlt=Base_Geo;
|
||||
Info.satUsed=Base_Used;
|
||||
Info.fixType=Base_Fix;
|
||||
|
||||
return true;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
35
dev/gps.h
Normal file
35
dev/gps.h
Normal file
@ -0,0 +1,35 @@
|
||||
#pragma once
|
||||
|
||||
struct Point { double Latitude; double Longitude; float Altitude; };
|
||||
|
||||
extern Point Base_BeginXYZ;
|
||||
|
||||
void GPS_Navigation(bool valid, ORI_Data& data, float p[3]);
|
||||
|
||||
float GPS_LocalDistance(Point p1, Point p2, float& dx, float& dy); // light formula
|
||||
float GPS_GlobalDistance(Point p1, Point p2); // Haversine formula
|
||||
|
||||
void GPS_Init();
|
||||
void GPS_Update();
|
||||
bool GPS_GetCoordinates(Point& Coord, bool& Valid);
|
||||
|
||||
struct GPS_BaseInfo
|
||||
{
|
||||
float LocalXYZ[3];
|
||||
float lat;
|
||||
float lon;
|
||||
float absAlt;
|
||||
float realAlt;
|
||||
float hdop;
|
||||
float vdop;
|
||||
float pdop;
|
||||
float noise;
|
||||
float jamming; //
|
||||
unsigned char satVisible;
|
||||
unsigned char satUsed;
|
||||
float speed;
|
||||
unsigned char fixType; // NO_GPS - 0, NO_FIX - 1, 2D_FIX - 2, 3D_FIX - 3, DGPS - 4, RTK_FLOAT - 5, RTK_FIXED - 6, STATIC - 7, PPP - 8
|
||||
unsigned long long timeUTC;
|
||||
};
|
||||
|
||||
bool GPS_GetBaseInfo(GPS_BaseInfo& Info);
|
||||
73
dev/ibus.cpp
Normal file
73
dev/ibus.cpp
Normal file
@ -0,0 +1,73 @@
|
||||
#include <string.h>
|
||||
|
||||
#include "uart.h"
|
||||
#include "tick.h"
|
||||
|
||||
#include "ibus.h"
|
||||
|
||||
// IBUS: LEN(1)+CMD(1)+DATA(0..32)+CHSM(2)
|
||||
|
||||
#define IBUS_LEN_MAX 32 // DATA(0..32)
|
||||
#define IBUS_LEN_PRO 3 // CMD(1)+CHSM(2)
|
||||
|
||||
void IBUS_Init(unsigned long Baud)
|
||||
{
|
||||
UART2_Init(Baud);
|
||||
}
|
||||
|
||||
static const unsigned long Size = IBUS_LEN_MAX + IBUS_LEN_PRO;
|
||||
static char Buffer[Size];
|
||||
|
||||
static char Pos = 0, Len = 0;
|
||||
static unsigned long CheckSum;
|
||||
|
||||
static unsigned long Time;
|
||||
|
||||
static bool Parse(IBUS_Data& Data, char byte)
|
||||
{
|
||||
unsigned long wait = TICK_GetCount() - Time;
|
||||
if (wait > 4) Len = 0; // Protocol synchronization lost !!!
|
||||
|
||||
if (!Len)
|
||||
{
|
||||
if (byte > IBUS_LEN_MAX || byte < IBUS_LEN_PRO) return false;
|
||||
Pos = 0;
|
||||
Len = byte - 1;
|
||||
CheckSum = 0xFFFFUL - byte;
|
||||
Time = TICK_GetCount();
|
||||
return false;
|
||||
}
|
||||
|
||||
Buffer[Pos++] = byte;
|
||||
if (Pos <= Len - 2) CheckSum -= byte;
|
||||
|
||||
if (Pos < Len) return false;
|
||||
|
||||
unsigned long len = Len;
|
||||
Len = 0;
|
||||
|
||||
unsigned short chsm = *(unsigned short*)(Buffer + (len - 2));
|
||||
|
||||
if (chsm != CheckSum) return false;
|
||||
|
||||
switch (Buffer[0])
|
||||
{
|
||||
case 0x40:
|
||||
{
|
||||
memcpy(&Data, Buffer + 1, len - 3 > sizeof(Data) ? sizeof(Data) : len - 3);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool IBUS_Update(IBUS_Data& Data, bool& Off)
|
||||
{
|
||||
char buf[Size];
|
||||
unsigned long size = UART2_Recv(buf, Size);
|
||||
|
||||
bool done = false;
|
||||
for (long a = 0; a < size; a++) done = Parse(Data, buf[a]);
|
||||
Off=Data.SWD<1000;
|
||||
return done;
|
||||
}
|
||||
15
dev/ibus.h
Normal file
15
dev/ibus.h
Normal file
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
|
||||
struct IBUS_Data // FlySky
|
||||
{
|
||||
short X, Y, Z, W;
|
||||
short SWA,SWB, SWC, SWD;
|
||||
short VRA, VRB;
|
||||
short OTHER[4];
|
||||
};
|
||||
|
||||
|
||||
void IBUS_Init(unsigned long Baud);
|
||||
bool IBUS_Update(IBUS_Data& Data, bool& Off);
|
||||
|
||||
|
||||
67
dev/imu.cpp
Normal file
67
dev/imu.cpp
Normal file
@ -0,0 +1,67 @@
|
||||
#include "i2c.h"
|
||||
|
||||
#include "imu.h"
|
||||
|
||||
static const unsigned char IMU_Addr = 0x6A; // ACC GYR
|
||||
|
||||
static inline short Rev16(short v)
|
||||
{
|
||||
asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
|
||||
return v;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static inline void IMU_SetReg(unsigned char Reg, unsigned char Value)
|
||||
{
|
||||
unsigned char reg[2];
|
||||
reg[0]=Reg; reg[1]=Value;
|
||||
I2C1_Write(IMU_Addr, reg, 2);
|
||||
I2C1_Stop();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static inline unsigned char IMU_GetReg(unsigned char Reg)
|
||||
{
|
||||
I2C1_Write(IMU_Addr, Reg);
|
||||
I2C1_Read(IMU_Addr, &Reg, 1);
|
||||
I2C1_Stop();
|
||||
return Reg;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void IMU_Init() //
|
||||
{
|
||||
I2C1_Init();
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
|
||||
unsigned char wai=IMU_GetReg(0x0F);
|
||||
|
||||
IMU_SetReg(0x10, 0x38);
|
||||
IMU_SetReg(0x11, 0x48);
|
||||
IMU_SetReg(0x12, 0x06);
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void IMU_Get(IMU_Data& Data)
|
||||
{
|
||||
struct {short temp; short gx; short gy; short gz; short ax; short ay; short az; } data;
|
||||
|
||||
I2C1_Write(IMU_Addr, 0x20);
|
||||
I2C1_Read(IMU_Addr, &data, sizeof(data));
|
||||
I2C1_Stop();
|
||||
|
||||
Data.Temp = 21+(((long)Rev16(data.temp))*128)/42735; // 21+(temperature / 333.87)
|
||||
|
||||
Data.Acc.X = Rev16(data.ax);
|
||||
Data.Acc.Y = Rev16(data.ay);
|
||||
Data.Acc.Z = Rev16(data.az);
|
||||
|
||||
Data.Gyr.X = Rev16(data.gx);
|
||||
Data.Gyr.Y = Rev16(data.gy);
|
||||
Data.Gyr.Z = Rev16(data.gz);
|
||||
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
15
dev/imu.h
Normal file
15
dev/imu.h
Normal file
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
|
||||
struct IMU_Data
|
||||
{
|
||||
short Temp;
|
||||
struct { short X, Y, Z; } Acc;
|
||||
struct { short X, Y, Z; } Gyr;
|
||||
struct { short X, Y, Z; } Mag;
|
||||
|
||||
float Bar;
|
||||
float Tmp;
|
||||
};
|
||||
|
||||
void IMU_Init();
|
||||
void IMU_Get(IMU_Data& Data);
|
||||
19
dev/led.cpp
Normal file
19
dev/led.cpp
Normal file
@ -0,0 +1,19 @@
|
||||
#include "stm32g4xx.h"
|
||||
|
||||
#include "gpio.h"
|
||||
|
||||
#include "led.h"
|
||||
|
||||
void LED_Init()
|
||||
{
|
||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||
GPIO_InitPin(GPIO_PIN_15 | GPIO_PORT_A | GPIO_OUTPUT);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void LED_Set(bool Set)
|
||||
{
|
||||
if (Set) GPIOA->BSRR = GPIO_BSRR_BS_15;
|
||||
else GPIOA->BSRR = GPIO_BSRR_BR_15;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
4
dev/led.h
Normal file
4
dev/led.h
Normal file
@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
void LED_Init();
|
||||
void LED_Set(bool Set);
|
||||
106
dev/mag.cpp
Normal file
106
dev/mag.cpp
Normal file
@ -0,0 +1,106 @@
|
||||
#include "i2c.h"
|
||||
|
||||
#include "mag.h"
|
||||
|
||||
static const unsigned char MAG_Addr = 0x0E; // IST8310
|
||||
|
||||
static void (*MAG_DoneProc)(MAG_Data& Data);
|
||||
|
||||
#pragma pack(push,1)
|
||||
struct MAG_DataBuffer {char s1; short mx; short my; short mz;};
|
||||
#pragma pack(pop)
|
||||
|
||||
static MAG_Data MagResult;
|
||||
|
||||
static bool Wait=false;
|
||||
|
||||
static void MAG_CallbackStart(unsigned char Address, const unsigned char* Data, unsigned char Size)
|
||||
{
|
||||
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static void MAG_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size)
|
||||
{
|
||||
MAG_DataBuffer* data=(MAG_DataBuffer*)Data;
|
||||
|
||||
if(data->s1 & 1)
|
||||
{
|
||||
MagResult.X = data->mx;
|
||||
MagResult.Y = data->my;
|
||||
MagResult.Z = data->mz;
|
||||
Wait=false;
|
||||
}
|
||||
|
||||
MAG_DoneProc(MagResult);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static unsigned char MAG_BufferStart[4], MAG_BufferDevice[8];
|
||||
static I2C_Request MAG_Start = {&MAG_CallbackStart, MAG_BufferStart, sizeof(MAG_BufferStart), 0};
|
||||
static I2C_Request MAG_Request = {&MAG_CallbackProc, MAG_BufferDevice, sizeof(MAG_BufferDevice), 0};
|
||||
|
||||
static inline void SetReg(unsigned char Reg, unsigned char Value)
|
||||
{
|
||||
unsigned char reg[2] = {Reg, Value}; // I2C_SLV4_ADDR - MAG_ADDR, Reg, I2C_SLVx_EN, Value
|
||||
I2C2_Write(MAG_Addr, reg, 2);
|
||||
I2C2_Stop();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void MAG_Init() // IST8310
|
||||
{
|
||||
I2C2_Init();
|
||||
I2C2_Init();
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); } // Подождем готовности устройства к работе
|
||||
|
||||
// Reset
|
||||
SetReg(11U, 1U); // CNTL2 <- SRST
|
||||
|
||||
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
||||
|
||||
// Config
|
||||
SetReg(11U, 8U); // CNTL2 <- DREN
|
||||
SetReg(65U, 8U | 1U); // AVGCNTL <- AVG_Y_AvgBy2 | AVG_XZ_AvgBy2
|
||||
SetReg(66U, 192U); // PDCNTL <- PULSE_DUR_Normal
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
bool MAG_Get(MAG_Data& Data)
|
||||
{
|
||||
if(!Wait) SetReg(10U, 1U); // CNTL1 <- MODE_SingleMeasurement
|
||||
Wait=true;
|
||||
|
||||
MAG_DataBuffer data;
|
||||
|
||||
I2C2_Write(MAG_Addr, 2U);
|
||||
I2C2_Read(MAG_Addr, &data, sizeof(MAG_DataBuffer));
|
||||
I2C2_Stop();
|
||||
|
||||
if(!(data.s1 & 1)) return false;
|
||||
|
||||
Data.X = data.mx;
|
||||
Data.Y = data.my;
|
||||
Data.Z = data.mz;
|
||||
|
||||
Wait=false;
|
||||
|
||||
return true;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void MAG_GetAsunc(void (*DoneProc)(MAG_Data& Data))
|
||||
{
|
||||
if(!Wait)
|
||||
{
|
||||
unsigned char reg[]={10U, 1U};
|
||||
I2C2_Trans(&MAG_Start, MAG_Addr, reg, 2, 0);
|
||||
Wait=true;
|
||||
}
|
||||
|
||||
MAG_DoneProc=DoneProc;
|
||||
unsigned char reg=2U;
|
||||
I2C2_Trans(&MAG_Request, MAG_Addr, ®, 1, sizeof(MAG_DataBuffer));
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
10
dev/mag.h
Normal file
10
dev/mag.h
Normal file
@ -0,0 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
struct MAG_Data
|
||||
{
|
||||
short X, Y, Z;
|
||||
};
|
||||
|
||||
void MAG_Init();
|
||||
bool MAG_Get(MAG_Data& Data);
|
||||
void MAG_GetAsunc(void (*DoneProc)(MAG_Data& Data));
|
||||
279
dev/ori.cpp
Normal file
279
dev/ori.cpp
Normal file
@ -0,0 +1,279 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "imu.h"
|
||||
#include "bar.h"
|
||||
|
||||
#include "med2.h"
|
||||
|
||||
#include "ori.h"
|
||||
#include "quat.h"
|
||||
|
||||
static const float PI = 3.14159265359f;
|
||||
static const float TO_DEG = 180.0f/PI;
|
||||
static const float TO_RAD = PI/180.0f;
|
||||
|
||||
static float FK_PR = 0.01f;
|
||||
static float FK_Y = 0.01f;
|
||||
|
||||
static float MEGA_BAR;
|
||||
static float MEGA_BAR_MIN=0.00001f;
|
||||
static float MEGA_BAR_MAX=0.1f;
|
||||
static float MEGA_BAR_MUL = 100.0f;
|
||||
static float MEGA_BAR_POW = 2.0f;
|
||||
static float MEGA_BAR_CNT = 7.0f;
|
||||
|
||||
ORI_Data DataORI;
|
||||
|
||||
const int BAR_MedCount=50;
|
||||
long BAR_MedData[BAR_MedCount];
|
||||
unsigned char BAR_MedIndex[BAR_MedCount];
|
||||
MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 20};
|
||||
|
||||
static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
|
||||
{
|
||||
static float falt=0;
|
||||
falt=falt*(1.0f-MEGA_BAR)+(Alt)*MEGA_BAR;
|
||||
|
||||
static float alt_speed=0, alt_acc=0;
|
||||
static int count=0;
|
||||
if(count>=MEGA_BAR_CNT)
|
||||
{
|
||||
static float last_b=0, last_s=0;
|
||||
alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
|
||||
alt_acc=(alt_speed-last_s);
|
||||
last_b=falt;
|
||||
last_s=alt_speed;
|
||||
count=0;
|
||||
}
|
||||
else count++;
|
||||
|
||||
float coef_w, coef_s;
|
||||
|
||||
coef_w=MEGA_BAR_MIN;
|
||||
float sub=fabsf(Alt-falt);
|
||||
coef_w*=powf(sub*MEGA_BAR_MUL, MEGA_BAR_POW);
|
||||
if(coef_w>MEGA_BAR_MAX) coef_w=MEGA_BAR_MAX;
|
||||
if(coef_w<MEGA_BAR_MIN) coef_w=MEGA_BAR_MIN;
|
||||
|
||||
|
||||
MEGA_BAR=coef_w;
|
||||
|
||||
//static float test_speed=0;
|
||||
//test_speed=(test_speed*15.0f+alt_speed)/16.0f;
|
||||
|
||||
Speed=MED2_Update(alt_speed*1000, BAR_Med)/1000.0f;
|
||||
|
||||
Acc=alt_acc;
|
||||
|
||||
return falt;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
/*const int BAR_MedCount=200;
|
||||
long BAR_MedData[BAR_MedCount];
|
||||
unsigned char BAR_MedIndex[BAR_MedCount];
|
||||
MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 100};
|
||||
|
||||
const int BAR_MedCount2=50;
|
||||
long BAR_MedData2[BAR_MedCount2];
|
||||
unsigned char BAR_MedIndex2[BAR_MedCount2];
|
||||
MED2_Data BAR_Med2 = {BAR_MedCount2, 0, BAR_MedIndex2, BAR_MedData2, 10};
|
||||
|
||||
static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
|
||||
{
|
||||
static float alt_speed=0, last_b=0;
|
||||
|
||||
float falt=MED2_Update(Alt*1000.0f, BAR_Med)/1000.0f;
|
||||
|
||||
static int count=0;
|
||||
if(count>=MEGA_BAR_CNT)
|
||||
{
|
||||
static float last_b=0;
|
||||
alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
|
||||
last_b=falt;
|
||||
count=0;
|
||||
}
|
||||
else count++;
|
||||
|
||||
Speed=MED2_Update(alt_speed*1000.0f, BAR_Med2)/1000.0f;
|
||||
|
||||
return falt;
|
||||
}*/
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
|
||||
|
||||
static float MEGA_ACC=0.01f;
|
||||
|
||||
static const long MEGA_Count=60; // 600ms at 100Hz
|
||||
static float MEGA_Alt_Buff[MEGA_Count];
|
||||
static float MEGA_Spd_Buff[MEGA_Count];
|
||||
static long MEGA_Index=0;
|
||||
|
||||
static float MegaFilterAcc(struct ORI_Data& Data)
|
||||
{
|
||||
float acc;
|
||||
|
||||
acc=fabsf(Data.Bar.Acc)*100.0f;
|
||||
float filt = MEGA_ACC;
|
||||
if(acc>1) filt/=acc;
|
||||
|
||||
acc=fabsf(Data.Iner.Z)*100.0f;
|
||||
if(acc>1) filt/=acc;
|
||||
|
||||
const float g=9.80665f;
|
||||
|
||||
static float i_speed=0;
|
||||
i_speed+=Data.Iner.Z*g/Data.Freq;
|
||||
|
||||
|
||||
MEGA_Alt_Buff[MEGA_Index++]=i_speed;
|
||||
if(MEGA_Index>=MEGA_Count) MEGA_Index=0;
|
||||
static float delay_speed;
|
||||
delay_speed=(delay_speed+MEGA_Alt_Buff[MEGA_Index])/2.0f;
|
||||
|
||||
float shift=Data.Bar.Speed-delay_speed;
|
||||
|
||||
for(int a=0; a<MEGA_Count; a++) MEGA_Alt_Buff[a] = MEGA_Alt_Buff[a]*(1.0f-filt) + (MEGA_Alt_Buff[a]+shift)*filt;
|
||||
|
||||
i_speed = i_speed*(1.0f-filt) + (i_speed+shift)*filt;
|
||||
|
||||
Data.Speed.Z=i_speed;
|
||||
|
||||
Data.Pos.Z+=i_speed/Data.Freq;
|
||||
|
||||
return 0;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void ORI_Init()
|
||||
{
|
||||
IMU_Init();
|
||||
MED2_Init(BAR_Med);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static void GetPos(struct ORI_Data& Data)
|
||||
{
|
||||
static long land=Data.Bar.Bar;
|
||||
float alt=0;
|
||||
alt = BAR_GetAltitude(land, Data.Bar.Bar);
|
||||
|
||||
float alt_speed, alt_acc;
|
||||
|
||||
float falt=MegaFilterAlt(Data.Freq, alt, alt_speed, alt_acc);
|
||||
|
||||
Data.Bar.Alt=alt;
|
||||
Data.Bar.Filt=falt;
|
||||
Data.Bar.Speed=alt_speed;
|
||||
Data.Bar.Acc=alt_acc;
|
||||
|
||||
MegaFilterAcc(Data);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//static short GyroShift[3]{-20, 30, 35};
|
||||
static short NormAcc_X[2]{-8250, 8130}, NormAcc_Y[2]{-8280, 8150}, NormAcc_Z[2]{-8250, 8330};
|
||||
//static short NormAcc_X[2]{-16500, 16496}, NormAcc_Y[2]{-16510, 16422}, NormAcc_Z[2]{-16410, 1672};
|
||||
//static short NormMag_X[2]{-270, 330}, NormMag_Y[2]{-340, 300}, NormMag_Z[2]{-380, 250}; // AK09916
|
||||
//static short NormMag_X[2]{-164, 170}, NormMag_Y[2]{-155, 150}, NormMag_Z[2]{-165, 148}; // IST8310
|
||||
static short NormMag_X[2]{-160, 160}, NormMag_Y[2]{-155, 140}, NormMag_Z[2]{-150, 140}; // IST8310
|
||||
//static short GyroShift[3]{52, 23, 17};
|
||||
static short GyroShift[3]{19, 20, 9};
|
||||
|
||||
static void NormalizeAcc(short& X, short& Y, short& Z, const short G)
|
||||
{
|
||||
short x=(NormAcc_X[1]-NormAcc_X[0])/2;
|
||||
short y=(NormAcc_Y[1]-NormAcc_Y[0])/2;
|
||||
short z=(NormAcc_Z[1]-NormAcc_Z[0])/2;
|
||||
|
||||
X=(X-(NormAcc_X[0]+x))*G/x;
|
||||
Y=(Y-(NormAcc_Y[0]+y))*G/y;
|
||||
Z=(Z-(NormAcc_Z[0]+z))*G/z;
|
||||
}
|
||||
|
||||
static void NormalizeMag(short& X, short& Y, short& Z, const short N)
|
||||
{
|
||||
short x=(NormMag_X[1]-NormMag_X[0])/2;
|
||||
short y=(NormMag_Y[1]-NormMag_Y[0])/2;
|
||||
short z=(NormMag_Z[1]-NormMag_Z[0])/2;
|
||||
|
||||
X=(X-(NormMag_X[0]+x))*N/x;
|
||||
Y=(Y-(NormMag_Y[0]+y))*N/y;
|
||||
Z=(Z-(NormMag_Z[0]+z))*N/z;
|
||||
}
|
||||
|
||||
void ORI_Get(ORI_Data& Data, const unsigned long Freq, IMU_Data& IMU, BAR_Data BAR)
|
||||
{
|
||||
Data.Freq=Freq;
|
||||
|
||||
short ax = IMU.Acc.X;
|
||||
short ay = IMU.Acc.Y;
|
||||
short az = IMU.Acc.Z;
|
||||
float G = 1000;
|
||||
NormalizeAcc(ax, ay, az, G);
|
||||
|
||||
Data.Acc.X = ax;
|
||||
Data.Acc.Y = ay;
|
||||
Data.Acc.Z = az;
|
||||
|
||||
short gx = IMU.Gyr.X - GyroShift[0];
|
||||
short gy = IMU.Gyr.Y - GyroShift[1];
|
||||
short gz = IMU.Gyr.Z - GyroShift[2];
|
||||
|
||||
Data.Gyr.X = gx;
|
||||
Data.Gyr.Y = gy;
|
||||
Data.Gyr.Z = gz;
|
||||
|
||||
short mx = 0;
|
||||
short my = 0;
|
||||
short mz = 0;
|
||||
|
||||
|
||||
Data.Mag.X = mx;
|
||||
Data.Mag.Y = my;
|
||||
Data.Mag.Z = mz;
|
||||
|
||||
static float bar_zero=0;
|
||||
static int zero_count=0;
|
||||
|
||||
if(zero_count<500)
|
||||
{
|
||||
bar_zero=BAR.Pressure;
|
||||
zero_count++;
|
||||
}
|
||||
|
||||
Data.Bar.Bar=BAR.Pressure;
|
||||
//Data.Bar.Alt=(bar_zero-BAR.Pressure)*9.0f;
|
||||
|
||||
Vec3 acc{((float)ax)/G, ((float)ay)/G, ((float)az)/G};
|
||||
Vec3 gyr{(float)gx, (float)gy, (float)gz};
|
||||
Vec3 mag{0.0f, 0.0f, 0.0f};
|
||||
ORI o = WorkAccGyroMag(acc, gyr, mag, 90, 0.01f);
|
||||
|
||||
static int test_pitch, test_roll;
|
||||
|
||||
Data.Pitch=o.Pitch;
|
||||
Data.Roll=o.Roll;
|
||||
Data.Yaw=o.Yaw;
|
||||
|
||||
Data.SinX=o.sinX;
|
||||
Data.SinY=o.sinY;
|
||||
Data.CosZ=o.cosZ;
|
||||
|
||||
Data.Iner.X=o.IneX;
|
||||
Data.Iner.Y=o.IneY;
|
||||
Data.Iner.Z=o.IneZ-1;
|
||||
|
||||
//Data.Speed.X+=o.IneX*9.8f/100.0f;
|
||||
//Data.Speed.Y+=o.IneY*9.8f/100.0f;
|
||||
//Data.Speed.Z+=(o.IneZ-1)*9.8f/100.0f;
|
||||
|
||||
Data.Temp.Acc=IMU.Temp;
|
||||
Data.Temp.Bar=BAR.Temp;
|
||||
|
||||
//GetAngle(Data, Freq);
|
||||
//GetCompass(Data);
|
||||
//GetIner(Data);
|
||||
GetPos(Data);
|
||||
}
|
||||
29
dev/ori.h
Normal file
29
dev/ori.h
Normal file
@ -0,0 +1,29 @@
|
||||
#pragma once
|
||||
|
||||
struct ORI_Data
|
||||
{
|
||||
unsigned long Freq;
|
||||
//---
|
||||
struct { short Acc; short Bar; } Temp;
|
||||
//---
|
||||
struct { short X, Y, Z; } Acc;
|
||||
struct { short X, Y, Z; } Gyr;
|
||||
struct { short X, Y, Z; } Mag;
|
||||
//---
|
||||
struct { float Bar, Alt, Filt, Speed, Acc; } Bar;
|
||||
//---
|
||||
bool Upside = false;
|
||||
float Pitch, Roll, Yaw;
|
||||
float SinX, SinY, CosZ;
|
||||
//---
|
||||
struct { float X, Y, Z; } Iner;
|
||||
struct { float X, Y, Z; } Speed;
|
||||
struct { float X, Y, Z; } Pos;
|
||||
};
|
||||
|
||||
extern ORI_Data DataORI;
|
||||
|
||||
void ORI_Init();
|
||||
void ORI_Get(ORI_Data& Data, const unsigned long Freq, struct IMU_Data& IMU, struct BAR_Data BAR);
|
||||
|
||||
void ORI_TiltCompAcc(float Pitch, float Roll, short Acc[3], ORI_Data& Data);
|
||||
206
dev/ori2.cpp
Normal file
206
dev/ori2.cpp
Normal file
@ -0,0 +1,206 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "imu.h"
|
||||
|
||||
#include "ori.h"
|
||||
|
||||
#include "med2.h"
|
||||
|
||||
static const float PI = 3.14159265359f;
|
||||
static const float TO_DEG = 57.2957795130f;
|
||||
|
||||
static float FK_PR = 0.01f;
|
||||
static float FK_Y = 0.01f;
|
||||
|
||||
void ORI_Init()
|
||||
{
|
||||
IMU_Init();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
static const long IMU_Iner_Fil = 64;
|
||||
static float IMU_Iner[3];
|
||||
|
||||
static float GetCompass(struct ORI_Data& Data)
|
||||
{
|
||||
float x=-Data.Mag.Y/1000.0f, y=-Data.Mag.X/1000.0f, z=-Data.Mag.Z/1000.0f;
|
||||
|
||||
float sp = Data.SinPitch;
|
||||
float sr = Data.SinRoll;
|
||||
|
||||
float cp = Data.CosPitch;
|
||||
float cr = Data.CosRoll;
|
||||
|
||||
float cx = x * cp + z * sp;
|
||||
float cy = x * sr * sp + y * cr - z * sr * cp;
|
||||
|
||||
float n=-atan2f(cy, cx)*TO_DEG;
|
||||
float north=360+n;
|
||||
|
||||
Data.North=Data.Upside;
|
||||
if(n>90) n=180-n;
|
||||
else if(n<-90) n=-180-n;
|
||||
else Data.North=!Data.Upside;
|
||||
|
||||
static long nnn;
|
||||
nnn=n;
|
||||
|
||||
Data.Yaw = Data.Yaw * (1.0f - FK_Y) + n * FK_Y;
|
||||
|
||||
return north;
|
||||
}
|
||||
|
||||
inline float GetAngle(float a1, float a2, float az)
|
||||
{
|
||||
if(a2 == 0.0f && az == 0.0f) return a1>0 ? 90.0f : -90.0f;
|
||||
return atanf(a1/(sqrtf(a2*a2+az*az))) * TO_DEG;
|
||||
}
|
||||
|
||||
static void GetAngle(ORI_Data& Data, const unsigned long Freq, short Iner[3])
|
||||
{
|
||||
const float sr=Data.SinRoll;
|
||||
const float sp=Data.SinPitch;
|
||||
const float cr=Data.CosRoll;
|
||||
const float cp=Data.CosPitch;
|
||||
|
||||
float gx = Data.Gyr.X / 16.0f / Freq;
|
||||
float gy = Data.Gyr.Y / 16.0f / Freq;
|
||||
float gz = Data.Gyr.Z / 16.0f / Freq;
|
||||
|
||||
float ax = Data.Acc.X;
|
||||
float ay = Data.Acc.Y;
|
||||
float az = Data.Acc.Z;
|
||||
|
||||
float sum_xyz;
|
||||
sum_xyz = sqrtf((ax*ax)+(ay*ay)+(az*az)) / 1000.0f;
|
||||
|
||||
Data.Acc.Sum=sum_xyz*1000;
|
||||
|
||||
if(Data.Upside) gz=-gz;
|
||||
|
||||
float gn=gz*cp*cr-gx*sr-gy*sp;
|
||||
|
||||
if(Data.Upside) { gx=-gx; gy=-gy; gz=-gz; }
|
||||
|
||||
float gp=gx*cr+gz*sr;
|
||||
float gr=gy*cp+gz*sp;
|
||||
|
||||
static float pitch=0;
|
||||
static float roll=0;
|
||||
float yaw=Data.Yaw;
|
||||
pitch -= gp;
|
||||
roll += gr;
|
||||
yaw += gn;
|
||||
|
||||
if(pitch>90.0f) { Data.Upside=!Data.Upside; pitch=180.0f-pitch; }
|
||||
else if(pitch<-90.0f) { Data.Upside=!Data.Upside; pitch=-180.0f-pitch; }
|
||||
|
||||
if(roll>90.0f) { Data.Upside=!Data.Upside; roll=180.0f-roll; }
|
||||
else if(roll<-90.0f) { Data.Upside=!Data.Upside; roll=-180.0f-roll; }
|
||||
|
||||
if(yaw>90.0f) { Data.North=!Data.North; yaw=180.0f-yaw; }
|
||||
else if(yaw<-90.0f) { Data.North=!Data.North; yaw=-180.0f-yaw; }
|
||||
|
||||
if((sum_xyz>0.98f) && (sum_xyz<1.02f))
|
||||
{
|
||||
float ap=GetAngle(ay, ax, az);
|
||||
float ar=GetAngle(ax, ay, az);
|
||||
|
||||
Data.Upside=az>0;
|
||||
pitch = pitch * (1.0f - FK_PR) + ap * FK_PR;
|
||||
roll = roll * (1.0f - FK_PR) + ar * FK_PR;
|
||||
}
|
||||
|
||||
Data.Pitch = pitch;
|
||||
Data.Roll = roll;
|
||||
Data.Yaw = yaw;
|
||||
|
||||
Data.SinPitch=sinf(pitch*PI/180.0f);
|
||||
Data.SinRoll=sinf(roll*PI/180.0f);
|
||||
Data.SinYaw=sinf(yaw*PI/180.0f);
|
||||
|
||||
Data.CosPitch=sqrtf(1.0f-Data.SinPitch*Data.SinPitch);
|
||||
Data.CosRoll=sqrtf(1.0f-Data.SinRoll*Data.SinRoll);
|
||||
Data.CosYaw=sqrtf(1.0f-Data.SinYaw*Data.SinYaw);
|
||||
}
|
||||
|
||||
static short GyroShift[3]{20, 10, -2};
|
||||
|
||||
static short NormAcc_X[2]{-4090, 4110}, NormAcc_Y[2]{-4130, 4075}, NormAcc_Z[2]{-4182, 4070};
|
||||
|
||||
static short NormMag_X[2]{-360, 270}, NormMag_Y[2]{-260, 340}, NormMag_Z[2]{-350, 320};
|
||||
|
||||
static void NormalizeAcc(short& X, short& Y, short& Z, const short G)
|
||||
{
|
||||
short x=(NormAcc_X[1]-NormAcc_X[0])/2;
|
||||
short y=(NormAcc_Y[1]-NormAcc_Y[0])/2;
|
||||
short z=(NormAcc_Z[1]-NormAcc_Z[0])/2;
|
||||
|
||||
X=(X-(NormAcc_X[0]+x))*G/x;
|
||||
Y=(Y-(NormAcc_Y[0]+y))*G/y;
|
||||
Z=(Z-(NormAcc_Z[0]+z))*G/z;
|
||||
}
|
||||
|
||||
static void NormalizeMag(short& X, short& Y, short& Z, const short N)
|
||||
{
|
||||
short x=(NormMag_X[1]-NormMag_X[0])/2;
|
||||
short y=(NormMag_Y[1]-NormMag_Y[0])/2;
|
||||
short z=(NormMag_Z[1]-NormMag_Z[0])/2;
|
||||
|
||||
X=(X-(NormMag_X[0]+x))*N/x;
|
||||
Y=(Y-(NormMag_Y[0]+y))*N/y;
|
||||
Z=(Z-(NormMag_Z[0]+z))*N/z;
|
||||
}
|
||||
|
||||
IMU_Data DataIMU;
|
||||
|
||||
void ORI_Get(ORI_Data& Data, const unsigned long Freq, short Iner[3])
|
||||
{
|
||||
IMU_Get(DataIMU);
|
||||
|
||||
short ax = DataIMU.Acc.X;
|
||||
short ay = DataIMU.Acc.Y;
|
||||
short az = DataIMU.Acc.Z;
|
||||
|
||||
NormalizeAcc(ax, ay, az, 1000);
|
||||
|
||||
Data.Acc.X = ax;
|
||||
Data.Acc.Y = ay;
|
||||
Data.Acc.Z = az;
|
||||
|
||||
Data.Gyr.X = DataIMU.Gyr.X + GyroShift[0];
|
||||
Data.Gyr.Y = DataIMU.Gyr.Y + GyroShift[1];
|
||||
Data.Gyr.Z = DataIMU.Gyr.Z + GyroShift[2];
|
||||
|
||||
short mx = DataIMU.Mag.X;
|
||||
short my = DataIMU.Mag.Y;
|
||||
short mz = DataIMU.Mag.Z;
|
||||
|
||||
NormalizeMag(mx, my, mz, 1000);
|
||||
|
||||
Data.Mag.X = mx;
|
||||
Data.Mag.Y = my;
|
||||
Data.Mag.Z = mz;
|
||||
|
||||
GetAngle(Data, Freq, Iner);
|
||||
GetCompass(Data);
|
||||
}
|
||||
|
||||
short CompAcc[3];
|
||||
|
||||
void ORI_TiltCompAcc(float Pitch, float Roll, short Acc[3], IMU_Data& Data)
|
||||
{
|
||||
float sin_r = sinf(Roll*PI/180.0f);
|
||||
float sin_p = sinf(Pitch*PI/180.0f);
|
||||
|
||||
float cos_r = sqrtf(1.0f-sin_r*sin_r); // cosf(Roll*PI/180.0f);
|
||||
float cos_p = sqrtf(1.0f-sin_p*sin_p); // cosf(Pitch*PI/180.0f);
|
||||
|
||||
Acc[0] = Data.Acc.X - 4000 * sin_r;
|
||||
Acc[1] = Data.Acc.Y + 4000 * sin_p;
|
||||
Acc[2] = Data.Acc.Z - 4000 * fabs(cos_r) * fabs(cos_p);
|
||||
|
||||
CompAcc[0]=Acc[0];
|
||||
CompAcc[1]=Acc[1];
|
||||
CompAcc[2]=Acc[2];
|
||||
}
|
||||
12
dev/sbus.h
Normal file
12
dev/sbus.h
Normal file
@ -0,0 +1,12 @@
|
||||
#pragma once
|
||||
|
||||
struct IBUS_Data // FlySky
|
||||
{
|
||||
short X, Y, Z, W;
|
||||
short SWA,SWB, SWC, SWD;
|
||||
short VRA, VRB;
|
||||
short OTHER[4];
|
||||
};
|
||||
|
||||
void IBUS_Init(unsigned long Baud);
|
||||
bool IBUS_Update(IBUS_Data& Data, bool& Off);
|
||||
193
dev/tele.cpp
Normal file
193
dev/tele.cpp
Normal file
@ -0,0 +1,193 @@
|
||||
#include <string.h>
|
||||
|
||||
#include "gpio.h"
|
||||
|
||||
#include "uart.h"
|
||||
#include "tick.h"
|
||||
|
||||
#include "tele.h"
|
||||
|
||||
enum class RecvModeEnum : unsigned char { Begin, Head, Titles, Data, Auto, SetPID, GetPID, Done };
|
||||
|
||||
struct RecvHead
|
||||
{
|
||||
RecvHead(){};
|
||||
RecvHead(RecvModeEnum mode, unsigned char size, unsigned char crc)
|
||||
{
|
||||
Mode = mode;
|
||||
DataSize = size;
|
||||
DataCRC8 = crc;
|
||||
CRC8 = (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
|
||||
}
|
||||
|
||||
RecvModeEnum Mode;
|
||||
unsigned char DataSize;
|
||||
unsigned char DataCRC8;
|
||||
|
||||
unsigned char CRC8;
|
||||
|
||||
bool Check()
|
||||
{
|
||||
return CRC8 == (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
|
||||
}
|
||||
};
|
||||
|
||||
unsigned char GetCRC8(const void* arr, int size)
|
||||
{
|
||||
unsigned char crc = 0;
|
||||
for (int a = 0; a < size; a++) crc ^= ((unsigned char*)arr)[a];
|
||||
return crc;
|
||||
}
|
||||
|
||||
static RecvHead Tele_Mode(RecvModeEnum::Begin,0,0);
|
||||
|
||||
void TELE_Init()
|
||||
{
|
||||
UART3_Init(57600);
|
||||
}
|
||||
|
||||
static unsigned long TELE_LastTime=0;
|
||||
static unsigned long TELE_WaitTimer=0;
|
||||
|
||||
static unsigned long TELE_Count=0;
|
||||
|
||||
static bool DataAuto=true;
|
||||
|
||||
void TELE_Update(const void* info, unsigned long size, unsigned long update)
|
||||
{
|
||||
|
||||
switch(Tele_Mode.Mode)
|
||||
{
|
||||
case RecvModeEnum::Begin:
|
||||
{
|
||||
unsigned char begin;
|
||||
int len=UART3_Recv(&begin, 1);
|
||||
TELE_WaitTimer=TICK_GetCount();
|
||||
if(len && begin==0xAA) Tele_Mode.Mode=RecvModeEnum::Head;
|
||||
break;
|
||||
}
|
||||
|
||||
case RecvModeEnum::Head:
|
||||
{
|
||||
int len=UART3_Recv(0, 0);
|
||||
if(len<sizeof(RecvHead)) break;
|
||||
|
||||
RecvHead head;
|
||||
UART3_Recv(&head, sizeof(RecvHead));
|
||||
|
||||
if(!head.Check()) Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
else Tele_Mode=head;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case RecvModeEnum::Titles:
|
||||
{
|
||||
const char* titles="Acc X|AccY|Acc Z|Acc S|Gyr X|Gyr Y|Gyr Z|Pitch|Roll|Yaw";
|
||||
int len=strlen(titles);
|
||||
|
||||
unsigned char h=0xAA;
|
||||
UART3_Send(&h, 1);
|
||||
|
||||
RecvHead head(RecvModeEnum::Titles, len, GetCRC8(titles, len));
|
||||
UART3_Send(&head, sizeof(head));
|
||||
UART3_Send(titles, len);
|
||||
|
||||
Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
/*case RecvModeEnum::GetPID:
|
||||
{
|
||||
float send[20]={PID_X.Min, PID_X.Max,
|
||||
|
||||
PID_X.P.Min, PID_X.P.Max, PID_X.P.C,
|
||||
PID_X.I.Min, PID_X.I.Max, PID_X.I.C,
|
||||
PID_X.D.Min, PID_X.D.Max, PID_X.D.C,
|
||||
|
||||
PID_Z.P.Min, PID_Z.P.Max, PID_Z.P.C,
|
||||
PID_Z.I.Min, PID_Z.I.Max, PID_Z.I.C,
|
||||
PID_Z.D.Min, PID_Z.D.Max, PID_Z.D.C
|
||||
};
|
||||
|
||||
unsigned char h=0xAA;
|
||||
UART3_Send(&h, 1);
|
||||
|
||||
RecvHead head(RecvModeEnum::GetPID, sizeof(send), GetCRC8(send, sizeof(send)));
|
||||
UART3_Send(&head, sizeof(head));
|
||||
UART3_Send(send, sizeof(send));
|
||||
|
||||
Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
|
||||
break;
|
||||
}*/
|
||||
|
||||
case RecvModeEnum::Data:
|
||||
{
|
||||
unsigned char h=0xAA;
|
||||
UART3_Send(&h, 1);
|
||||
|
||||
RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
|
||||
UART3_Send(&head, sizeof(head));
|
||||
UART3_Send(info, size);
|
||||
|
||||
Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
/*case RecvModeEnum::SetPID:
|
||||
{
|
||||
float recv[20];
|
||||
|
||||
unsigned long len=UART3_Recv(0, 0);
|
||||
if(len<sizeof(recv)) break;
|
||||
|
||||
UART3_Recv(recv, sizeof(recv));
|
||||
|
||||
unsigned char crc=GetCRC8(recv, sizeof(recv));
|
||||
if(Tele_Mode.DataCRC8!=crc) break;
|
||||
|
||||
PID_X.Min=recv[0]; PID_X.Max=recv[1];
|
||||
|
||||
PID_X.P.Min=recv[2]; PID_X.P.Max=recv[3]; PID_X.P.C=recv[4];
|
||||
PID_X.I.Min=recv[5]; PID_X.I.Max=recv[6]; PID_X.I.C=recv[7];
|
||||
PID_X.D.Min=recv[8]; PID_X.D.Max=recv[9]; PID_X.D.C=recv[10];
|
||||
|
||||
PID_Z.P.Min=recv[11]; PID_Z.P.Max=recv[12]; PID_Z.P.C=recv[13];
|
||||
PID_Z.I.Min=recv[14]; PID_Z.I.Max=recv[15]; PID_Z.I.C=recv[16];
|
||||
PID_Z.D.Min=recv[17]; PID_Z.D.Max=recv[18]; PID_Z.D.C=recv[19];
|
||||
|
||||
unsigned char h=0xAA;
|
||||
UART3_Send(&h, 1);
|
||||
|
||||
RecvHead head(RecvModeEnum::Done, 0, 0);
|
||||
UART3_Send(&head, sizeof(head));
|
||||
|
||||
Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
|
||||
break;
|
||||
}*/
|
||||
|
||||
default:
|
||||
{
|
||||
Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if(TICK_GetCount()-TELE_WaitTimer>100) Tele_Mode.Mode=RecvModeEnum::Begin;
|
||||
|
||||
if(DataAuto && (TICK_GetCount()-TELE_LastTime>update))
|
||||
{
|
||||
TELE_LastTime=TICK_GetCount();
|
||||
|
||||
unsigned char h=0xAA;
|
||||
UART3_Send(&h, 1);
|
||||
|
||||
RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
|
||||
UART3_Send(&head, sizeof(head));
|
||||
UART3_Send(info, size);
|
||||
}
|
||||
}
|
||||
4
dev/tele.h
Normal file
4
dev/tele.h
Normal file
@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
void TELE_Init();
|
||||
void TELE_Update(const void* info, unsigned long size, unsigned long update);
|
||||
122
dev/tof.cpp
Normal file
122
dev/tof.cpp
Normal file
@ -0,0 +1,122 @@
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "gpio.h"
|
||||
|
||||
#include "uart.h"
|
||||
#include "tick.h"
|
||||
|
||||
#include "tof.h"
|
||||
|
||||
#pragma pack(push,1)
|
||||
struct HeadTOF
|
||||
{
|
||||
unsigned short Header; // 0x5959
|
||||
unsigned short Distance;
|
||||
unsigned short Strength;
|
||||
unsigned short Temprature; // Temprature/8 - 256;
|
||||
unsigned char Check; // Summ & 0xFF
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
static const unsigned long TOF_Count=8;
|
||||
static char TOF_Buffer[TOF_Count];
|
||||
static unsigned long TOF_Size=0;
|
||||
|
||||
static float TOF_Range=0;
|
||||
|
||||
void TOF_Init()
|
||||
{
|
||||
UART3_Init(115200);
|
||||
}
|
||||
|
||||
void TOF_Update()
|
||||
{
|
||||
char c;
|
||||
if(UART3_Recv(&c, 1))
|
||||
{
|
||||
if(c==' ')
|
||||
{
|
||||
TOF_Size=1;
|
||||
TOF_Buffer[0]=c;
|
||||
return;
|
||||
}
|
||||
|
||||
if(!TOF_Size) return;
|
||||
|
||||
if(c=='\r')
|
||||
{
|
||||
if(TOF_Size<4)
|
||||
{
|
||||
TOF_Size=0;
|
||||
return;
|
||||
}
|
||||
|
||||
TOF_Buffer[TOF_Size]='\0';
|
||||
|
||||
char* end;
|
||||
float range=strtof(TOF_Buffer, &end);
|
||||
if(end-TOF_Buffer==TOF_Size) TOF_Range=range;
|
||||
else
|
||||
range=0;
|
||||
TOF_Size=0;
|
||||
return;
|
||||
}
|
||||
|
||||
TOF_Buffer[TOF_Size++]=c;
|
||||
|
||||
if(TOF_Size>=TOF_Count)
|
||||
{
|
||||
TOF_Size=0;
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void TOF_Update2()
|
||||
{
|
||||
static HeadTOF buffer;
|
||||
static unsigned long index=0;
|
||||
char* data=(char*)&buffer;
|
||||
|
||||
char c;
|
||||
while(UART3_Recv(&c, 1))
|
||||
{
|
||||
if(index<2)
|
||||
{
|
||||
if(c==0x59)
|
||||
{
|
||||
data[index++]=c;
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
index=0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
data[index++]=c;
|
||||
|
||||
if(index<sizeof(HeadTOF)) continue;
|
||||
|
||||
index=0;
|
||||
|
||||
unsigned char check=0;
|
||||
for(int a=0; a<sizeof(HeadTOF)-1; a++) check+=data[a];
|
||||
|
||||
if(buffer.Check!=check) break;
|
||||
|
||||
TOF_Range=((float)buffer.Distance)/100;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float TOF_GetRange()
|
||||
{
|
||||
return TOF_Range;
|
||||
}
|
||||
Reference in New Issue
Block a user