moved some readonly strings to flash, thanks zyp
cleaned up frsky telemetry a bit with verbose telemetry ID names, changed loop timing a bit, added output for heading and voltage (when vbat is enabled). telemetry stuff is untested, so someone try it out. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@226 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com
parent
005308b430
commit
193902079c
4545
obj/baseflight.hex
4545
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
|
|
@ -30,7 +30,7 @@ static float _atof(const char *p);
|
|||
static char *ftoa(float x, char *floatString);
|
||||
|
||||
// sync this with MultiType enum from mw.h
|
||||
const char *mixerNames[] = {
|
||||
const char * const mixerNames[] = {
|
||||
"TRI", "QUADP", "QUADX", "BI",
|
||||
"GIMBAL", "Y6", "HEX6",
|
||||
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
|
||||
|
|
@ -38,7 +38,7 @@ const char *mixerNames[] = {
|
|||
};
|
||||
|
||||
// sync this with AvailableFeatures enum from board.h
|
||||
const char *featureNames[] = {
|
||||
const char * const featureNames[] = {
|
||||
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
|
||||
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
|
||||
"FAILSAFE", "SONAR", "TELEMETRY",
|
||||
|
|
@ -46,12 +46,12 @@ const char *featureNames[] = {
|
|||
};
|
||||
|
||||
// sync this with AvailableSensors enum from board.h
|
||||
const char *sensorNames[] = {
|
||||
const char * const sensorNames[] = {
|
||||
"ACC", "BARO", "MAG", "SONAR", "GPS", NULL
|
||||
};
|
||||
|
||||
//
|
||||
const char *accNames[] = {
|
||||
const char * const accNames[] = {
|
||||
"", "ADXL345", "MPU6050", "MMA845x", NULL
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -5,13 +5,13 @@
|
|||
#define sq(x) ((x)*(x))
|
||||
#endif
|
||||
|
||||
uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
|
||||
const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
|
||||
|
||||
static void GPS_NewData(uint16_t c);
|
||||
static void GPS_set_pids(void);
|
||||
static void gpsPrint(const char *str);
|
||||
|
||||
static const char *gpsInitStrings[] = {
|
||||
static const char * const gpsInitStrings[] = {
|
||||
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // UBX0..3
|
||||
"$PUBX,41,1,0003,0001,38400,0*26\r\n",
|
||||
"$PUBX,41,1,0003,0001,57600,0*2D\r\n",
|
||||
|
|
|
|||
132
src/telemetry.c
132
src/telemetry.c
|
|
@ -4,49 +4,101 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
#define CYCLETIME 200
|
||||
#define CYCLETIME 125
|
||||
|
||||
#define PROTOCOL_HEADER 0x5E
|
||||
#define PROTOCOL_TAIL 0x5E
|
||||
|
||||
// Data Ids (bp = before decimal point; af = after decimal point)
|
||||
// Official data IDs
|
||||
#define ID_GPS_ALTIDUTE_BP 0x01
|
||||
#define ID_GPS_ALTIDUTE_AP 0x09
|
||||
#define ID_TEMPRATURE1 0x02
|
||||
#define ID_RPM 0x03
|
||||
#define ID_FUEL_LEVEL 0x04
|
||||
#define ID_TEMPRATURE2 0x05
|
||||
#define ID_VOLT 0x06
|
||||
#define ID_ALTITUDE_BP 0x10
|
||||
#define ID_ALTITUDE_AP 0x21
|
||||
#define ID_GPS_SPEED_BP 0x11
|
||||
#define ID_GPS_SPEED_AP 0x19
|
||||
#define ID_LONGITUDE_BP 0x12
|
||||
#define ID_LONGITUDE_AP 0x1A
|
||||
#define ID_E_W 0x22
|
||||
#define ID_LATITUDE_BP 0x13
|
||||
#define ID_LATITUDE_AP 0x1B
|
||||
#define ID_N_S 0x23
|
||||
#define ID_COURSE_BP 0x14
|
||||
#define ID_COURSE_AP 0x1C
|
||||
#define ID_DATE_MONTH 0x15
|
||||
#define ID_YEAR 0x16
|
||||
#define ID_HOUR_MINUTE 0x17
|
||||
#define ID_SECOND 0x18
|
||||
#define ID_ACC_X 0x24
|
||||
#define ID_ACC_Y 0x25
|
||||
#define ID_ACC_Z 0x26
|
||||
#define ID_VOLTAGE_AMP_BP 0x3A
|
||||
#define ID_VOLTAGE_AMP_AP 0x3B
|
||||
#define ID_CURRENT 0x28
|
||||
// User defined data IDs
|
||||
#define ID_GYRO_X 0x40
|
||||
#define ID_GYRO_Y 0x41
|
||||
#define ID_GYRO_Z 0x42
|
||||
|
||||
static void sendDataHead(uint8_t id)
|
||||
{
|
||||
uartWrite(0x5E);
|
||||
uartWrite(PROTOCOL_HEADER);
|
||||
uartWrite(id);
|
||||
}
|
||||
|
||||
static void sendTelemetryTail(void)
|
||||
{
|
||||
uartWrite(0x5E);
|
||||
uartWrite(PROTOCOL_TAIL);
|
||||
}
|
||||
|
||||
static void serializeFrsky(uint8_t data)
|
||||
{
|
||||
// take care of byte stuffing
|
||||
if (data == 0x5e) {
|
||||
uartWrite(0x5d);
|
||||
uartWrite(0x3e);
|
||||
} else if (data == 0x5d) {
|
||||
uartWrite(0x5d);
|
||||
uartWrite(0x3d);
|
||||
} else
|
||||
uartWrite(data);
|
||||
}
|
||||
|
||||
static void serialize16(int16_t a)
|
||||
{
|
||||
uint8_t t;
|
||||
t = a;
|
||||
uartWrite(t);
|
||||
serializeFrsky(t);
|
||||
t = a >> 8 & 0xff;
|
||||
uartWrite(t);
|
||||
serializeFrsky(t);
|
||||
}
|
||||
|
||||
static void sendAccel(void)
|
||||
{
|
||||
uint8_t i;
|
||||
int i;
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
sendDataHead(0x24 + i);
|
||||
sendDataHead(ID_ACC_X + i);
|
||||
serialize16(((float)accSmooth[i] / acc_1G) * 1000);
|
||||
}
|
||||
}
|
||||
|
||||
static void sendBaro(void)
|
||||
{
|
||||
sendDataHead(0x10);
|
||||
sendDataHead(ID_ALTITUDE_BP);
|
||||
serialize16(EstAlt / 100);
|
||||
sendDataHead(0x21);
|
||||
sendDataHead(ID_ALTITUDE_AP);
|
||||
serialize16(EstAlt % 100);
|
||||
}
|
||||
|
||||
static void sendTemperature1(void)
|
||||
{
|
||||
sendDataHead(0x02);
|
||||
sendDataHead(ID_TEMPRATURE1);
|
||||
serialize16(telemTemperature1 / 10);
|
||||
}
|
||||
|
||||
|
|
@ -56,30 +108,50 @@ static void sendTime(void)
|
|||
uint8_t minutes = (seconds / 60) % 60;
|
||||
|
||||
// if we fly for more than an hour, something's wrong anyway
|
||||
sendDataHead(0x17);
|
||||
sendDataHead(ID_HOUR_MINUTE);
|
||||
serialize16(minutes << 8);
|
||||
sendDataHead(0x18);
|
||||
sendDataHead(ID_SECOND);
|
||||
serialize16(seconds % 60);
|
||||
}
|
||||
|
||||
static void sendGPS(void)
|
||||
{
|
||||
sendDataHead(0x13);
|
||||
sendDataHead(ID_LATITUDE_BP);
|
||||
serialize16(abs(GPS_coord[LAT]) / 100000);
|
||||
sendDataHead(0x13 + 8);
|
||||
sendDataHead(ID_LATITUDE_AP);
|
||||
serialize16((abs(GPS_coord[LAT]) / 100000) % 10000);
|
||||
|
||||
sendDataHead(0x1B + 8);
|
||||
sendDataHead(ID_N_S);
|
||||
serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');
|
||||
|
||||
sendDataHead(0x12);
|
||||
sendDataHead(ID_LONGITUDE_BP);
|
||||
serialize16(abs(GPS_coord[LON]) / 100000);
|
||||
sendDataHead(0x12 + 8);
|
||||
sendDataHead(ID_LONGITUDE_AP);
|
||||
serialize16((abs(GPS_coord[LON]) / 100000) % 10000);
|
||||
sendDataHead(0x1A + 8);
|
||||
sendDataHead(ID_E_W);
|
||||
serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
|
||||
}
|
||||
|
||||
static void sendVoltage(void)
|
||||
{
|
||||
uint16_t voltage;
|
||||
|
||||
voltage = (vbat * 110) / 21;
|
||||
|
||||
sendDataHead(ID_VOLTAGE_AMP_BP);
|
||||
serialize16(voltage / 100);
|
||||
sendDataHead(ID_VOLTAGE_AMP_AP);
|
||||
serialize16(((voltage % 100) + 5) / 10);
|
||||
}
|
||||
|
||||
static void sendHeading(void)
|
||||
{
|
||||
sendDataHead(ID_COURSE_BP);
|
||||
serialize16(heading);
|
||||
sendDataHead(ID_COURSE_AP);
|
||||
serialize16(0);
|
||||
}
|
||||
|
||||
static bool telemetryEnabled = false;
|
||||
|
||||
void initTelemetry(bool State)
|
||||
|
|
@ -102,20 +174,26 @@ void sendTelemetry(void)
|
|||
lastCycleTime = millis();
|
||||
cycleNum++;
|
||||
|
||||
// Frame 1: sent every 200ms
|
||||
// Sent every 125ms
|
||||
sendAccel();
|
||||
sendBaro();
|
||||
sendTemperature1();
|
||||
sendTelemetryTail();
|
||||
|
||||
if ((cycleNum % 5) == 0) { // Frame 2: Sent every 1s
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
sendGPS();
|
||||
sendTelemetryTail();
|
||||
}
|
||||
if ((cycleNum % 4) == 0) { // Sent every 500ms
|
||||
sendBaro();
|
||||
sendHeading();
|
||||
sendTelemetryTail();
|
||||
}
|
||||
|
||||
if (cycleNum == 25) { //Frame 3: Sent every 5s
|
||||
if ((cycleNum % 8) == 0) { // Sent every 1s
|
||||
sendTemperature1();
|
||||
if (feature(FEATURE_VBAT))
|
||||
sendVoltage();
|
||||
if (sensors(SENSOR_GPS))
|
||||
sendGPS();
|
||||
sendTelemetryTail();
|
||||
}
|
||||
|
||||
if (cycleNum == 40) { //Frame 3: Sent every 5s
|
||||
cycleNum = 0;
|
||||
sendTime();
|
||||
sendTelemetryTail();
|
||||
|
|
|
|||
Loading…
Reference in New Issue