Skip to content

Commit e4e461d

Browse files
cemizmcemizm
committed
APS-M Align Support
1 parent 89b23fe commit e4e461d

16 files changed

Lines changed: 237 additions & 14 deletions

File tree

‎Firmware/Pro/include/Board.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
#include <Timer.h>
1919
#include <I2C.h>
2020

21-
#define FIRMWARE_VERSION 0x02010200
21+
#define FIRMWARE_VERSION 0x02020100
2222
#define HARDWARE_VERSION 0x01
2323

2424
namespace Board

‎Firmware/Releases/AnySensePro.bin‎

1.17 KB
Binary file not shown.

‎Firmware/libs/mavlink/common.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,9 @@
7373
<entry value="22" name="MAV_AUTOPILOT_TAROT_ZYX_M">
7474
<description>Tarot ZYX-M</description>
7575
</entry>
76+
<entry value="23" name="MAV_AUTOPILOT_ALIGN_APS_M">
77+
<description>Align APS-M</description>
78+
</entry>
7679
</enum>
7780
<enum name="MAV_TYPE">
7881
<entry value="0" name="MAV_TYPE_GENERIC">

‎Firmware/libs/mavlink/onboard/AnySense/version.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
#ifndef MAVLINK_VERSION_H
66
#define MAVLINK_VERSION_H
77

8-
#define MAVLINK_BUILD_DATE "Sun Mar 6 18:21:01 2016"
8+
#define MAVLINK_BUILD_DATE "Mon Jul 4 09:38:17 2016"
99
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
1010
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
1111

‎Firmware/libs/mavlink/onboard/common/common.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,8 @@ typedef enum MAV_AUTOPILOT
6262
MAV_AUTOPILOT_WOOKONG=20, /* DJI Wookong M | */
6363
MAV_AUTOPILOT_A2=21, /* DJI A2 | */
6464
MAV_AUTOPILOT_TAROT_ZYX_M=22, /* Tarot ZYX-M | */
65-
MAV_AUTOPILOT_ENUM_END=23, /* | */
65+
MAV_AUTOPILOT_ALIGN_APS_M=23, /* Align APS-M | */
66+
MAV_AUTOPILOT_ENUM_END=24, /* | */
6667
} MAV_AUTOPILOT;
6768
#endif
6869

‎Firmware/libs/mavlink/onboard/common/version.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
#ifndef MAVLINK_VERSION_H
66
#define MAVLINK_VERSION_H
77

8-
#define MAVLINK_BUILD_DATE "Sun Mar 6 18:21:04 2016"
8+
#define MAVLINK_BUILD_DATE "Mon Jul 4 09:38:20 2016"
99
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
1010
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
1111

‎Firmware/shared/Application/include/FCAdapter.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ class FCAdapter
2424

2525
enum Protocol
2626
{
27-
MAVLink = 0, Tarot = 1, Last = 2,
27+
MAVLink = 0, Tarot = 1, Align = 2, Last = 3,
2828
};
2929

3030
FCAdapter() :

‎Firmware/shared/Application/include/FCAlign.h‎

Lines changed: 63 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,11 +23,65 @@ class FCAlign: public FCAdapter, public HAL::InterruptHandler
2323
{
2424

2525
private:
26-
static const uint8_t TmpReceiveBufferSize = 32;
26+
static const uint8_t TmpReceiveBufferSize = 16;
2727
static const uint8_t ReceiveBufferSize = 128;
28-
static const uint8_t PaketSize = 64;
29-
static const uint8_t PacketContentSize = 38;
30-
static const uint16_t StartSign = 0x6BD2;
28+
static const uint8_t StartSign = 0x41;
29+
30+
enum StatusFlag
31+
{
32+
None = 0,
33+
Feets = 1,
34+
Flag2 = 2,
35+
AlarmBattery = 4,
36+
AlarmDistance = 8,
37+
AlarmAltitudeHigh = 16,
38+
AlarmAltitudeLow = 32,
39+
AlarmSatellites = 64,
40+
GPSFound = 128
41+
};
42+
43+
enum AlignMode
44+
{
45+
AlignModeManual = 1,
46+
AlignModeAttitude = 2,
47+
AlignModeGPSVelocity = 3,
48+
AlignModeIOC = 4,
49+
AlignModeGPSAngular = 5,
50+
AlignModeFailsafeVelocity = 19,
51+
AlignModeFailsafeAngular = 21,
52+
};
53+
54+
enum PaketType {
55+
PaketTypeOSD = 0x01,
56+
PaketTypeUnknown1 = 0x02,
57+
};
58+
59+
struct Paket
60+
{
61+
uint8_t StartByte;
62+
PaketType Type;
63+
uint8_t PaketSize;
64+
65+
StatusFlag Status;
66+
AlignMode Mode;
67+
uint8_t Satellites;
68+
uint8_t Voltage;
69+
int16_t Course;
70+
int16_t Pitch;
71+
int16_t Roll;
72+
int16_t Altitude;
73+
int32_t Longitude;
74+
int32_t Latitude;
75+
int16_t Speed;
76+
77+
uint8_t unk2[4];
78+
79+
uint8_t VerticalVelocity;
80+
uint16_t Time;
81+
uint16_t Distance;
82+
uint8_t Checksum;
83+
}__attribute__((packed, aligned(1)));
84+
3185

3286
HAL::USART& m_usart;
3387
OSAL::EventFlag gotMsg;
@@ -37,6 +91,11 @@ class FCAlign: public FCAdapter, public HAL::InterruptHandler
3791
uint8_t m_receiveIndex;
3892
uint8_t m_processIndex;
3993

94+
void Process(Paket& paket);
95+
96+
97+
uint8_t calculateChecksum(const uint8_t* data, uint8_t size);
98+
4099
public:
41100
FCAlign(HAL::USART& usart) :
42101
FCAdapter(), m_usart(usart), m_tmpReceiveBuffer(), m_tmpReceiveIndex(0), m_receiveBuffer(), m_receiveIndex(0), m_processIndex(

‎Firmware/shared/Application/include/SensorStore.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ namespace App
2121

2222
enum FCType
2323
{
24-
Unknown = 0, Naza = 1, Phantom = 2, Wookong = 3, A2 = 4, APM = 5, ZYX_M = 6,
24+
Unknown = 0, Naza = 1, Phantom = 2, Wookong = 3, A2 = 4, APM = 5, ZYX_M = 6, Align = 7,
2525
};
2626

2727
enum GPSFixType

‎Firmware/shared/Application/source/FCAlign.cpp‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,156 @@
66
*/
77

88
#include "FCAlign.h"
9+
#include <string.h>
10+
#include "Endianess.h"
11+
#include "SensorStore.h"
12+
13+
extern uint8_t PriorityFC;
914

1015
namespace App
1116
{
1217

18+
static_assert (sizeof(FCAlign) <= FCAdapter::Workspace, "FCAlign will not fit!");
19+
20+
void FCAlign::Init()
21+
{
22+
23+
static_assert (sizeof(FCAlign::Paket) == 35, "Align PaketSize does not match!");
24+
25+
m_usart.Init(GPIO_PUPD_PULLDOWN);
26+
m_usart.SetBaudrate(115200);
27+
m_usart.SetStopbits(USART_STOPBITS_1);
28+
m_usart.SetDatabits(8);
29+
m_usart.SetFlowControl(USART_FLOWCONTROL_NONE);
30+
m_usart.SetParity(USART_PARITY_NONE);
31+
m_usart.SetMode(USART_MODE_RX);
32+
m_usart.DisableOverrunDetection();
33+
34+
HAL::InterruptRegistry.Enable(m_usart.NVIC_IRQn, PriorityFC, this);
35+
36+
m_usart.EnableRxInterrupt();
37+
m_usart.Enable();
38+
}
39+
40+
void FCAlign::Run()
41+
{
42+
Paket* tmpPaket;
43+
volatile uint16_t checksum = 0;
44+
45+
while (IsAlive())
46+
{
47+
if (gotMsg.wait(WaitForDataTimeout))
48+
{
49+
tmpPaket = (Paket*) &m_receiveBuffer[m_processIndex];
50+
51+
if (tmpPaket->StartByte != StartSign || tmpPaket->Type != PaketTypeOSD)
52+
{
53+
//Find next Start sign
54+
for (; m_processIndex < m_receiveIndex; m_processIndex++)
55+
{
56+
tmpPaket = (Paket*) &m_receiveBuffer[m_processIndex];
57+
if (tmpPaket->StartByte == StartSign && tmpPaket->Type == PaketTypeOSD)
58+
break;
59+
}
60+
}
61+
62+
if (tmpPaket->StartByte == StartSign && tmpPaket->Type == PaketTypeOSD)
63+
{
64+
if ((m_receiveIndex - m_processIndex) >= (tmpPaket->PaketSize + 4))
65+
{
66+
checksum = calculateChecksum((const uint8_t*) tmpPaket, tmpPaket->PaketSize + 3);
67+
if (checksum == tmpPaket->Checksum)
68+
{
69+
Process(*tmpPaket);
70+
m_processIndex += tmpPaket->PaketSize + 4;
71+
}
72+
else
73+
m_processIndex += 1;
74+
75+
}
76+
}
77+
78+
if ((m_receiveIndex + TmpReceiveBufferSize) > ReceiveBufferSize)
79+
{
80+
m_receiveIndex = m_receiveIndex - m_processIndex;
81+
memmove(m_receiveBuffer, &m_receiveBuffer[m_processIndex], m_receiveIndex);
82+
m_processIndex = 0;
83+
memset(&m_receiveBuffer[m_receiveIndex], 0, ReceiveBufferSize - m_receiveIndex);
84+
}
85+
86+
}
87+
}
88+
89+
DeInit();
90+
}
91+
92+
void FCAlign::Process(Paket& paket)
93+
{
94+
SetHeartbeat();
95+
96+
SensorData.SetFCType(FCType::Align);
97+
98+
SensorData.SetSensorPresent(Sensors::SGPS, 1);
99+
SensorData.SetSensorPresent(Sensors::GForce, 1);
100+
SensorData.SetSensorPresent(Sensors::Vario, 1);
101+
SensorData.SetSensorPresent(Sensors::Voltage, 1);
102+
103+
SensorData.SetRoll(paket.Roll / 10.f);
104+
SensorData.SetPitch(paket.Pitch / 10.f);
105+
106+
SensorData.SetPositionCurrent(paket.Latitude / 10000000.f, paket.Longitude / 10000000.f);
107+
SensorData.SetAltitude(paket.Altitude / 10.f);
108+
SensorData.SetVerticalSpeed(paket.VerticalVelocity / 10.f);
109+
SensorData.SetSpeed(paket.Speed / 10.f);
110+
SensorData.SetBattery(paket.Voltage * 100);
111+
112+
if (paket.Mode == AlignModeManual)
113+
SensorData.SetFlightMode(FlightMode::Manual);
114+
else if (paket.Mode == AlignModeAttitude)
115+
SensorData.SetFlightMode(FlightMode::Attitude);
116+
else if (paket.Mode == AlignModeGPSVelocity || paket.Mode == AlignModeIOC || paket.Mode == AlignModeGPSAngular)
117+
SensorData.SetFlightMode(FlightMode::GPS);
118+
else if (paket.Mode == AlignModeFailsafeVelocity || paket.Mode == AlignModeFailsafeAngular)
119+
SensorData.SetFlightMode(FlightMode::Failsafe);
120+
121+
SensorData.SetSatellites(paket.Satellites);
122+
SensorData.SetFixType(
123+
paket.Satellites == 0 ? GPSFixType::FixNo : paket.Satellites < 4 ? GPSFixType::Fix2D : GPSFixType::Fix3D);
124+
}
125+
126+
uint8_t FCAlign::calculateChecksum(const uint8_t* data, uint8_t size)
127+
{
128+
uint8_t sum = 0x00;
129+
130+
for (uint8_t j = 0; j < size; j++)
131+
sum += data[j];
132+
133+
return sum;
134+
}
135+
136+
void FCAlign::DeInit(void)
137+
{
138+
m_usart.DisableRxInterrupt();
139+
m_usart.Disable();
140+
141+
HAL::InterruptRegistry.Disable(m_usart.NVIC_IRQn);
142+
143+
m_usart.DeInit();
144+
}
145+
146+
void FCAlign::ISR(void)
147+
{
148+
if (m_usart.GetInterruptSource(USART_ISR_RXNE))
149+
{
150+
m_tmpReceiveBuffer[m_tmpReceiveIndex++] = m_usart.Receive();
151+
if (m_tmpReceiveIndex == TmpReceiveBufferSize)
152+
{
153+
memcpy(&m_receiveBuffer[m_receiveIndex], m_tmpReceiveBuffer, TmpReceiveBufferSize);
154+
m_receiveIndex += TmpReceiveBufferSize;
155+
m_tmpReceiveIndex = 0;
156+
gotMsg.signal_isr();
157+
}
158+
}
159+
}
160+
13161
} /* namespace App */

0 commit comments

Comments
 (0)