2 ******************************************************************************
3 * @addtogroup OpenPilotModules OpenPilot Modules
5 * @addtogroup OSDOUTPUTModule OSDOutput Module
6 * @brief On screen display support
10 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
11 * @brief Interfacing with OSD module
12 * @see The GNU Public License (GPL) Version 3
14 *****************************************************************************/
16 * This program is free software; you can redistribute it and/or modify
17 * it under the terms of the GNU General Public License as published by
18 * the Free Software Foundation; either version 3 of the License, or
19 * (at your option) any later version.
21 * This program is distributed in the hope that it will be useful, but
22 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
23 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
26 * You should have received a copy of the GNU General Public License along
27 * with this program; if not, write to the Free Software Foundation, Inc.,
28 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
31 #include "openpilot.h"
33 #if FLIGHTBATTERYSTATE_SUPPORTED
34 #include "flightbatterystate.h"
37 #if POSITIONACTUAL_SUPPORTED
38 #include "positionstate.h"
41 #include "systemalarms.h"
42 #include "attitudestate.h"
43 #include "hwsettings.h"
44 #include "flightstatus.h"
46 #include <pios_board_io.h>
48 static bool osdoutputEnabled
;
51 OSD_HK_SYNC_A
= 0xCB, OSD_HK_SYNC_B
= 0x34,
54 enum osd_hk_pkt_type
{
55 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,
58 enum osd_hk_control_mode
{
59 OSD_HK_CONTROL_MODE_MANUAL
= 0, OSD_HK_CONTROL_MODE_STABILIZED
= 1, OSD_HK_CONTROL_MODE_AUTO
= 2,
62 struct osd_hk_blob_misc
{
63 uint8_t type
; /* Always OSD_HK_PKT_TYPE_MISC */
66 // uint16_t home; /* Big Endian */
67 enum osd_hk_control_mode control_mode
;
69 uint16_t current
; /* Big Endian */
70 } __attribute__((packed
));
72 struct osd_hk_blob_att
{
73 uint8_t type
; /* Always OSD_HK_PKT_TYPE_ATT */
77 int16_t speed
; /* Big Endian */
78 } __attribute__((packed
));
80 struct osd_hk_blob_nav
{
81 uint8_t type
; /* Always OSD_HK_PKT_TYPE_NAV */
82 uint32_t gps_lat
; /* Big Endian */
83 uint32_t gps_lon
; /* Big Endian */
84 } __attribute__((packed
));
86 struct osd_hk_blob_maint
{
87 uint8_t type
; /* Always OSD_HK_PKT_TYPE_MAINT */
89 uint16_t gps_alt
; /* Big Endian */
90 uint16_t gps_dis
; /* Big Endian */
94 } __attribute__((packed
));
96 struct osd_hk_blob_mode
{
97 uint8_t type
; /* Always OSD_HK_PKT_TYPE_MODE */
99 uint16_t gps_alt
; /* Big Endian */
100 uint16_t gps_dis
; /* Big Endian */
104 } __attribute__((packed
));
106 union osd_hk_pkt_blobs
{
107 struct osd_hk_blob_misc misc
;
108 struct osd_hk_blob_nav nav
;
109 struct osd_hk_blob_maint maint
;
110 struct osd_hk_blob_att att
;
111 struct osd_hk_blob_mode mode
;
112 } __attribute__((packed
));
115 enum osd_hk_sync sync
;
116 enum osd_hk_pkt_type t
;
117 union osd_hk_pkt_blobs v
;
118 } __attribute__((packed
));
120 static struct osd_hk_msg osd_hk_msg_buf
;
122 static volatile bool newPositionStateData
= false;
123 static volatile bool newBattData
= false;
124 static volatile bool newAttitudeData
= false;
125 static volatile bool newAlarmData
= false;
127 static uint32_t osd_hk_com_id
;
128 static uint8_t osd_hk_msg_dropped
;
129 static uint8_t osd_packet
;
131 static void send_update(__attribute__((unused
)) UAVObjEvent
*ev
)
133 static enum osd_hk_sync sync
= OSD_HK_SYNC_A
;
135 struct osd_hk_msg
*msg
= &osd_hk_msg_buf
;
136 union osd_hk_pkt_blobs
*blob
= &(osd_hk_msg_buf
.v
);
138 /* Make sure we have a COM port bound */
139 if (!osd_hk_com_id
) {
143 FlightStatusData flightStatus
;
150 switch (osd_packet
) {
151 case OSD_HK_PKT_TYPE_MISC
:
153 case OSD_HK_PKT_TYPE_NAV
:
155 case OSD_HK_PKT_TYPE_MAINT
:
157 case OSD_HK_PKT_TYPE_ATT
:
158 msg
->t
= OSD_HK_PKT_TYPE_ATT
;
160 AttitudeStateRollGet(&roll
);
161 blob
->att
.roll
= (int16_t)(roll
* 10);
164 AttitudeStatePitchGet(&pitch
);
165 blob
->att
.pitch
= (int16_t)(pitch
* 10);
168 AttitudeStateYawGet(&yaw
);
169 blob
->att
.yaw
= (int16_t)(yaw
* 10);
171 case OSD_HK_PKT_TYPE_MODE
:
172 msg
->t
= OSD_HK_PKT_TYPE_MODE
;
173 FlightStatusGet(&flightStatus
);
174 blob
->mode
.fltmode
= flightStatus
.FlightMode
;
175 blob
->mode
.armed
= flightStatus
.Armed
;
181 /* Field not supported yet */
182 // blob->misc.control_mode = 0;
183 /*if (newAlarmData) {
184 SystemAlarmsData alarms;
185 SystemAlarmsGet(&alarms);
187 switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) {
188 case SYSTEMALARMS_ALARM_UNINITIALISED:
189 case SYSTEMALARMS_ALARM_OK:
190 blob->misc.low_battery = 0;
192 case SYSTEMALARMS_ALARM_WARNING:
193 case SYSTEMALARMS_ALARM_ERROR:
194 case SYSTEMALARMS_ALARM_CRITICAL:
196 blob->misc.low_battery = 1;
200 newAlarmData = false;
203 #if FLIGHTBATTERYSUPPORTED
205 float consumed_energy
;
206 FlightBatteryStateConsumedEnergyGet(&consumed_energy
);
208 uint16_t current
= (uint16_t)(consumed_energy
* 10);
210 /* convert to big endian */
211 blob
->misc
.current
= (
212 (current
& 0xFF00 >> 8) |
213 (current
& 0x00FF << 8));
218 // blob->misc.current = 0;
221 #if POSITIONACTUAL_SUPPORTED
222 if (newPositionStateData
) {
223 PositionStateData position
;
224 PositionStateGet(&position
);
226 /* compute 3D distance */
228 pow(position
.North
, 2) +
229 pow(position
.East
, 2) +
230 pow(position
.Down
, 2));
231 /* convert from cm to dm (10ths of m) */
232 uint16_t home
= (uint16_t)(d
/ 10);
234 /* convert to big endian */
236 (home
& 0xFF00 >> 8) |
237 (home
& 0x00FF << 8));
239 newPositionStateData
= false;
242 // blob->misc.home = 0;
243 #endif /* if POSITIONACTUAL_SUPPORTED */
245 if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id
, (uint8_t *)&osd_hk_msg_buf
, sizeof(osd_hk_msg_buf
))) {
246 /* Sent a packet, flip to the opposite sync */
247 if (sync
== OSD_HK_SYNC_A
) {
248 sync
= OSD_HK_SYNC_B
;
250 sync
= OSD_HK_SYNC_A
;
253 /* Failed to send this update */
254 osd_hk_msg_dropped
++;
257 if (osd_packet
> OSD_HK_PKT_TYPE_MODE
) {
258 osd_packet
= OSD_HK_PKT_TYPE_MISC
;
262 static UAVObjEvent ev
;
264 static int32_t osdoutputStart(void)
266 if (osdoutputEnabled
) {
267 /* Start a periodic timer to kick sending of an update */
268 EventPeriodicCallbackCreate(&ev
, send_update
, 25 / portTICK_RATE_MS
);
274 static int32_t osdoutputInitialize(void)
276 osd_hk_com_id
= PIOS_COM_OSDHK
;
277 #ifdef MODULE_OSDOUTPUT_BUILTIN
278 osdoutputEnabled
= 1;
280 HwSettingsOptionalModulesData optionalModules
;
281 HwSettingsOptionalModulesGet(&optionalModules
);
282 if (optionalModules
.OsdHk
== HWSETTINGS_OPTIONALMODULES_ENABLED
) {
283 osdoutputEnabled
= 1;
285 osdoutputEnabled
= 0;
289 if (osdoutputEnabled
&& osd_hk_com_id
) {
290 PIOS_COM_ChangeBaud(osd_hk_com_id
, 57600);
295 MODULE_INITCALL(osdoutputInitialize
, osdoutputStart
);