1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/flight/modules/Osd/osdoutput/osdoutput.c
Richard Flay (Hyper) a2d8544931 OP-931: adds -Wextra compiler option for the flight code, and makes the bazillion code changes required
to make the flight code compile again. Needs careful review, particularly all the fixes for the
signed vs unsigned comparisons.

+review OPReview-459
2013-05-05 16:32:24 +09:30

305 lines
7.9 KiB
C

/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup OSDOUTPUTModule OSDOutput Module
* @brief On screen display support
* @{
*
* @file osdoutput.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Interfacing with OSD 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"
#if FLIGHTBATTERYSTATE_SUPPORTED
#include "flightbatterystate.h"
#endif
#if POSITIONACTUAL_SUPPORTED
#include "positionactual.h"
#endif
#include "systemalarms.h"
#include "attitudeactual.h"
#include "hwsettings.h"
#include "flightstatus.h"
static bool osdoutputEnabled;
enum osd_hk_sync
{
OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34,
};
enum osd_hk_pkt_type
{
OSD_HK_PKT_TYPE_MISC = 0, OSD_HK_PKT_TYPE_NAV = 1, OSD_HK_PKT_TYPE_MAINT = 2, OSD_HK_PKT_TYPE_ATT = 3, OSD_HK_PKT_TYPE_MODE = 4,
};
enum osd_hk_control_mode
{
OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2,
};
struct osd_hk_blob_misc
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */
int16_t roll;
int16_t pitch;
//uint16_t home; /* Big Endian */
enum osd_hk_control_mode control_mode;
uint8_t low_battery;
uint16_t current; /* Big Endian */
}__attribute__((packed));
struct osd_hk_blob_att
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */
int16_t roll;
int16_t pitch;
int16_t yaw;
int16_t speed; /* Big Endian */
}__attribute__((packed));
struct osd_hk_blob_nav
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */
uint32_t gps_lat; /* Big Endian */
uint32_t gps_lon; /* Big Endian */
}__attribute__((packed));
struct osd_hk_blob_maint
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */
uint8_t gps_speed;
uint16_t gps_alt; /* Big Endian */
uint16_t gps_dis; /* Big Endian */
uint8_t status;
uint8_t config;
uint8_t emerg;
}__attribute__((packed));
struct osd_hk_blob_mode
{
uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */
uint8_t fltmode;
uint16_t gps_alt; /* Big Endian */
uint16_t gps_dis; /* Big Endian */
uint8_t armed;
uint8_t config;
uint8_t emerg;
}__attribute__((packed));
union osd_hk_pkt_blobs
{
struct osd_hk_blob_misc misc;
struct osd_hk_blob_nav nav;
struct osd_hk_blob_maint maint;
struct osd_hk_blob_att att;
struct osd_hk_blob_mode mode;
}__attribute__((packed));
struct osd_hk_msg
{
enum osd_hk_sync sync;
enum osd_hk_pkt_type t;
union osd_hk_pkt_blobs v;
}__attribute__((packed));
static struct osd_hk_msg osd_hk_msg_buf;
static volatile bool newPositionActualData = false;
static volatile bool newBattData = false;
static volatile bool newAttitudeData = false;
static volatile bool newAlarmData = false;
static uint32_t osd_hk_com_id;
static uint8_t osd_hk_msg_dropped;
static uint8_t osd_packet;
static void send_update(__attribute__((unused)) UAVObjEvent * ev)
{
static enum osd_hk_sync sync = OSD_HK_SYNC_A;
struct osd_hk_msg * msg = &osd_hk_msg_buf;
union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v);
/* Make sure we have a COM port bound */
if (!osd_hk_com_id) {
return;
}
FlightStatusData flightStatus;
/*
* Set up the message
*/
msg->sync = sync;
switch (osd_packet) {
case OSD_HK_PKT_TYPE_MISC:
break;
case OSD_HK_PKT_TYPE_NAV:
break;
case OSD_HK_PKT_TYPE_MAINT:
break;
case OSD_HK_PKT_TYPE_ATT:
msg->t = OSD_HK_PKT_TYPE_ATT;
float roll;
AttitudeActualRollGet(&roll);
blob->att.roll = (int16_t)(roll * 10);
float pitch;
AttitudeActualPitchGet(&pitch);
blob->att.pitch = (int16_t)(pitch * 10);
float yaw;
AttitudeActualYawGet(&yaw);
blob->att.yaw = (int16_t)(yaw * 10);
break;
case OSD_HK_PKT_TYPE_MODE:
msg->t = OSD_HK_PKT_TYPE_MODE;
FlightStatusGet(&flightStatus);
blob->mode.fltmode = flightStatus.FlightMode;
blob->mode.armed = flightStatus.Armed;
break;
default:
break;
}
/* Field not supported yet */
//blob->misc.control_mode = 0;
/*if (newAlarmData) {
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) {
case SYSTEMALARMS_ALARM_UNINITIALISED:
case SYSTEMALARMS_ALARM_OK:
blob->misc.low_battery = 0;
break;
case SYSTEMALARMS_ALARM_WARNING:
case SYSTEMALARMS_ALARM_ERROR:
case SYSTEMALARMS_ALARM_CRITICAL:
default:
blob->misc.low_battery = 1;
break;
}
newAlarmData = false;
}*/
#if FLIGHTBATTERYSUPPORTED
if (newBattData) {
float consumed_energy;
FlightBatteryStateConsumedEnergyGet(&consumed_energy);
uint16_t current = (uint16_t)(consumed_energy * 10);
/* convert to big endian */
blob->misc.current = (
(current & 0xFF00 >> 8) |
(current & 0x00FF << 8));
newBattData = false;
}
#else
//blob->misc.current = 0;
#endif
#if POSITIONACTUAL_SUPPORTED
if (newPositionActualData) {
PositionActualData position;
PositionActualGet(&position);
/* compute 3D distance */
float d = sqrt(
pow(position.North,2) +
pow(position.East,2) +
pow(position.Down,2));
/* convert from cm to dm (10ths of m) */
uint16_t home = (uint16_t)(d / 10);
/* convert to big endian */
blob->misc.home = (
(home & 0xFF00 >> 8) |
(home & 0x00FF << 8));
newPositionActualData = false;
}
#else
//blob->misc.home = 0;
#endif
if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *) &osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) {
/* Sent a packet, flip to the opposite sync */
if (sync == OSD_HK_SYNC_A) {
sync = OSD_HK_SYNC_B;
} else {
sync = OSD_HK_SYNC_A;
}
} else {
/* Failed to send this update */
osd_hk_msg_dropped++;
}
osd_packet++;
if (osd_packet > OSD_HK_PKT_TYPE_MODE) {
osd_packet = OSD_HK_PKT_TYPE_MISC;
}
}
static UAVObjEvent ev;
static int32_t osdoutputStart(void)
{
if (osdoutputEnabled) {
/* Start a periodic timer to kick sending of an update */
EventPeriodicCallbackCreate(&ev, send_update, 25 / portTICK_RATE_MS);
return 0;
}
return -1;
}
static int32_t osdoutputInitialize(void)
{
osd_hk_com_id = PIOS_COM_OSDHK;
#ifdef MODULE_OSDOUTPUT_BUILTIN
osdoutputEnabled = 1;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
osdoutputEnabled = 1;
} else {
osdoutputEnabled = 0;
}
#endif
return 0;
}
MODULE_INITCALL(osdoutputInitialize, osdoutputStart)
/**
* @}
* @}
*/