rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-12 20:08:48 +01:00
Code Issues Releases Activity
45f456580f
LibrePilot/flight/modules/Osd/OsdEtStd/OsdEtStd.c

558 lines
13 KiB
C
Raw Blame History

/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup OSDModule OSD Module
* @brief On screen display support
* @{
*
* @file OsdEtStd.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Interfacing with EagleTree OSD Std module
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "flightbatterystate.h"
#include "gpsposition.h"
#include "attitudeactual.h"
#include "baroaltitude.h"
//
// Configuration
//
#define DEBUG_PORT PIOS_COM_GPS
//#define ENABLE_DEBUG_MSG
//#define PIOS_ENABLE_DEBUG_PINS
//#define DUMP_CONFIG // Enable this do read and dump the OSD config
//
// Private constants
//
#ifdef ENABLE_DEBUG_MSG
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
#else
#define DEBUG_MSG(format, ...)
#endif
#define CONFIG_LENGTH 6726
#define MIN(a,b) ((a)<(b)?(a):(b))
#define SUPPORTED_VERSION 115
#define OSD_ADDRESS 0x30
#define OSDMSG_V_LS_IDX 10
#define OSDMSG_BALT_IDX1 11
#define OSDMSG_BALT_IDX2 12
#define OSDMSG_A_LS_IDX 17
#define OSDMSG_VA_MS_IDX 18
#define OSDMSG_LAT_IDX 33
#define OSDMSG_LON_IDX 37
#define OSDMSG_HOME_IDX 47
#define OSDMSG_ALT_IDX 49
#define OSDMSG_NB_SATS 58
#define OSDMSG_GPS_STAT 59
#define OSDMSG_GPS_STAT_NOFIX 0x03
#define OSDMSG_GPS_STAT_FIX 0x2B
#define OSDMSG_GPS_STAT_HB_FLAG 0x10
#ifdef PIOS_ENABLE_DEBUG_PINS
#define DEBUG_PIN_RUNNING 0
#define DEBUG_PIN_I2C 1
#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
#define DebugPinLow(x) PIOS_DEBUG_PinLow(x)
#else
#define DebugPinHigh(x)
#define DebugPinLow(x)
#endif
static const char *UpdateConfFilePath = "/etosd/update.ocf";
#ifdef DUMP_CONFIG
static const char *DumpConfFilePath = "/etosd/dump.ocf";
#endif
//
// Private types
//
//
// Private variables
//
// | Header / cmd?| | V | E3 | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat|
// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
uint8_t msg[63] =
{ 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90, 0x00,
0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A };
static volatile bool newPosData = FALSE;
static volatile bool newBattData = FALSE;
static volatile bool newBaroData = FALSE;
static enum
{
STATE_DETECT,
STATE_UPDATE_CONF,
STATE_DUMP_CONF,
STTE_RUNNING
} state;
static UAVObjEvent ev;
static uint32_t version = 0;
//
// Private functions
//
static void WriteToMsg8(uint8_t index, uint8_t value)
{
if (value > 100)
value = 100;
msg[index] = ((value / 10) << 4) + (value % 10);
}
static void WriteToMsg16(uint8_t index, uint16_t value)
{
WriteToMsg8(index, value % 100);
WriteToMsg8(index + 1, value / 100);
}
static void WriteToMsg24(uint8_t index, uint32_t value)
{
WriteToMsg16(index, value % 10000);
WriteToMsg8(index + 2, value / 10000);
}
static void WriteToMsg32(uint8_t index, uint32_t value)
{
WriteToMsg16(index, value % 10000);
WriteToMsg16(index + 2, value / 10000);
}
static void SetCoord(uint8_t index, uint32_t coord)
{
#define E7 10000000
uint8_t deg = coord / E7;
float sec = (float)(coord - deg*E7) / ((float)E7/(60.0*10000));
WriteToMsg8(index + 3, deg);
WriteToMsg24(index, sec);
}
static void SetCourse(uint16_t dir)
{
WriteToMsg16(OSDMSG_HOME_IDX, dir);
}
static void SetBaroAltitude(int16_t altitudeMeter)
{
// calculated formula
// ET OSD uses first update as zeropoint and then +- from that
altitudeMeter=(4571-altitudeMeter)/0.37;
msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter&0x00FF);
msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8)&0x3F;
}
static void SetAltitude(uint32_t altitudeMeter)
{
WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10);
}
static void SetVoltage(uint32_t milliVolt)
{
msg[OSDMSG_VA_MS_IDX] &= 0x0F;
msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4;
msg[OSDMSG_V_LS_IDX] = (milliVolt % 6444) * 256 / 6444;
}
static void SetCurrent(uint32_t milliAmp)
{
uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA;
msg[OSDMSG_VA_MS_IDX] &= 0xF0;
msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F);
msg[OSDMSG_A_LS_IDX] = (value & 0xFF);
}
static void SetNbSats(uint8_t nb)
{
msg[OSDMSG_NB_SATS] = nb;
}
static void FlightBatteryStateUpdatedCb(UAVObjEvent * ev)
{
newBattData = TRUE;
}
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
{
newPosData = TRUE;
}
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
{
newBaroData = TRUE;
}
static bool Read(uint32_t start, uint8_t length, uint8_t * buffer)
{
uint8_t cmd[5];
const struct pios_i2c_txn txn_list[] = {
{
.addr = OSD_ADDRESS,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(cmd),
.buf = cmd,
}
,
{
.addr = OSD_ADDRESS,
.rw = PIOS_I2C_TXN_READ,
.len = length,
.buf = buffer,
}
,
};
cmd[0] = 0x02;
cmd[1] = 0x05;
cmd[2] = (uint8_t) (start & 0xFF);
cmd[3] = (uint8_t) (start >> 8);
cmd[4] = length;
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0;
}
static bool Write(uint32_t start, uint8_t length, const uint8_t * buffer)
{
uint8_t cmd[125];
uint8_t ack[3];
const struct pios_i2c_txn txn_list1[] = {
{
.addr = OSD_ADDRESS,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(cmd),
.buf = cmd,
}
,
{
.addr = OSD_ADDRESS,
.rw = PIOS_I2C_TXN_READ,
.len = sizeof(ack),
.buf = ack,
}
,
};
if (length + 5 > sizeof(cmd)) {
// Too big
return FALSE;
}
cmd[0] = 0x01;
cmd[1] = 0x7D;
cmd[2] = (uint8_t) (start & 0xFF);
cmd[3] = (uint8_t) (start >> 8);
cmd[4] = length;
memcpy(&cmd[5], buffer, length);
ack[0] = 0;
//
// FIXME: See OP-305, the driver seems to return FALSE while all is OK
//
PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1));
// if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) {
//DEBUG_MSG("ACK=%d ", ack[0]);
if (ack[0] == 49) {
return TRUE;
}
// }
return FALSE;
}
static uint32_t ReadSwVersion(void)
{
uint8_t buf[4];
uint32_t version;
if (Read(0, 4, buf)) {
version = (buf[0] - '0') * 100;
version += (buf[2] - '0') * 10;
version += (buf[3] - '0');
} else {
version = 0;
}
return version;
}
static void UpdateConfig(void)
{
uint8_t buf[120];
uint32_t addr = 0;
uint32_t n;
FILEINFO file;
uint32_t res;
// Try to open the file that contains a new config
res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file);
if (res == DFS_OK) {
uint32_t bytesRead;
bool ok = TRUE;
DEBUG_MSG("Updating Config ");
// Write the config-data in blocks to OSD
while (addr < CONFIG_LENGTH && ok) {
n = MIN(CONFIG_LENGTH - addr, sizeof(buf));
res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n);
if (res == DFS_OK && bytesRead == n) {
ok = Write(addr, n, buf);
if (ok) {
//DEBUG_MSG(" %d %d\n", addr, n);
DEBUG_MSG(".");
addr += n;
}
} else {
DEBUG_MSG(" FILEREAD FAILED ");
}
}
DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
// If writing is OK, read the data back and verify
if (ok) {
DEBUG_MSG("Verify Config ");
DFS_Seek(&file, 0, PIOS_SDCARD_Sector);
addr = 0;
while (addr < CONFIG_LENGTH && ok) {
// First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file
n = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2);
ok = Read(addr, n, buf);
if (ok) {
uint32_t bytesRead;
res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n);
if (res == DFS_OK && bytesRead == n) {
DEBUG_MSG(".");
addr += n;
if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) {
DEBUG_MSG(" MISMATCH ");
ok = FALSE;
}
}
}
}
DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
}
DFS_Close(&file);
// When the config was updated correctly, remove the config-file
if (ok) {
DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) UpdateConfFilePath, PIOS_SDCARD_Sector);
}
}
}
static void DumpConfig(void)
{
#ifdef DUMP_CONFIG
uint8_t buf[100];
uint32_t addr = 0;
uint32_t n;
FILEINFO file;
uint32_t res;
DEBUG_MSG("Dumping Config ");
res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file);
if (res == DFS_OK) {
uint32_t bytesWritten;
bool ok = TRUE;
while (addr < CONFIG_LENGTH && ok) {
n = MIN(CONFIG_LENGTH - addr, sizeof(buf));
ok = Read(addr, n, buf);
if (ok) {
res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n);
if (res == DFS_OK && bytesWritten == n) {
//DEBUG_MSG(" %d %d\n", addr, n);
DEBUG_MSG(".");
addr += n;
}
}
}
DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
DFS_Close(&file);
} else {
DEBUG_MSG("Error Opening File %x\n", res);
}
#endif
}
static void Run(void)
{
static uint32_t cnt = 0;
if (newBattData) {
FlightBatteryStateData flightBatteryData;
FlightBatteryStateGet(&flightBatteryData);
//DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000));
SetVoltage((uint32_t) (flightBatteryData.Voltage * 1000));
SetCurrent((uint32_t) (flightBatteryData.Current * 1000));
newBattData = FALSE;
}
if (newPosData) {
GPSPositionData positionData;
AttitudeActualData attitudeActualData;
GPSPositionGet(&positionData);
AttitudeActualGet(&attitudeActualData);
//DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt,
// positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude);
// GPS Status
if (positionData.Status == GPSPOSITION_STATUS_FIX3D)
msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX;
else
msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX;
msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG;
// GPS info
SetCoord(OSDMSG_LAT_IDX, positionData.Latitude);
SetCoord(OSDMSG_LON_IDX, positionData.Longitude);
SetAltitude(positionData.Altitude);
SetNbSats(positionData.Satellites);
SetCourse(attitudeActualData.Yaw);
newPosData = FALSE;
} else {
msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG;
}
if (newBaroData) {
BaroAltitudeData baroData;
BaroAltitudeGet(&baroData);
SetBaroAltitude(baroData.Altitude);
newBaroData = FALSE;
}
DEBUG_MSG("SendMsg %d\n",cnt);
{
DebugPinHigh(DEBUG_PIN_I2C);
const struct pios_i2c_txn txn_list[] = {
{
.addr = OSD_ADDRESS,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(msg),
.buf = msg,
}
,
};
PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
DebugPinLow(DEBUG_PIN_I2C);
}
cnt++;
}
static void onTimer(UAVObjEvent * ev)
{
DebugPinHigh(DEBUG_PIN_RUNNING);
#ifdef ENABLE_DEBUG_MSG
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
#endif
if (state == STATE_DETECT) {
version = ReadSwVersion();
DEBUG_MSG("SW: %d ", version);
if (version == SUPPORTED_VERSION) {
DEBUG_MSG("OK\n");
state++;
} else {
DEBUG_MSG("INVALID\n");
}
} else if (state == STATE_UPDATE_CONF) {
UpdateConfig();
state++;
} else if (state == STATE_DUMP_CONF) {
DumpConfig();
state++;
} else if (state == STTE_RUNNING) {
Run();
} else {
// should not happen..
state = STATE_DETECT;
}
DebugPinLow(DEBUG_PIN_RUNNING);
}
//
// Public functions
//
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OsdEtStdInitialize(void)
{
GPSPositionConnectCallback(GPSPositionUpdatedCb);
FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb);
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
memset(&ev,0,sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS);
return 0;
}
Reference in New Issue View Git Blame Copy Permalink