Merged in f5soh/librepilot/update_credits (pull request #529)
[librepilot.git] / flight / modules / Osd / osdoutput / osdoutput.c
blob76de3d5f8adecbd91e2c200391b82f52ef5febca
1 /**
2 ******************************************************************************
3 * @addtogroup OpenPilotModules OpenPilot Modules
4 * @{
5 * @addtogroup OSDOUTPUTModule OSDOutput Module
6 * @brief On screen display support
7 * @{
9 * @file osdoutput.c
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
24 * for more details.
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"
35 #endif
37 #if POSITIONACTUAL_SUPPORTED
38 #include "positionstate.h"
39 #endif
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;
50 enum osd_hk_sync {
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 */
64 int16_t roll;
65 int16_t pitch;
66 // uint16_t home; /* Big Endian */
67 enum osd_hk_control_mode control_mode;
68 uint8_t low_battery;
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 */
74 int16_t roll;
75 int16_t pitch;
76 int16_t yaw;
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 */
88 uint8_t gps_speed;
89 uint16_t gps_alt; /* Big Endian */
90 uint16_t gps_dis; /* Big Endian */
91 uint8_t status;
92 uint8_t config;
93 uint8_t emerg;
94 } __attribute__((packed));
96 struct osd_hk_blob_mode {
97 uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */
98 uint8_t fltmode;
99 uint16_t gps_alt; /* Big Endian */
100 uint16_t gps_dis; /* Big Endian */
101 uint8_t armed;
102 uint8_t config;
103 uint8_t emerg;
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));
114 struct osd_hk_msg {
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) {
140 return;
143 FlightStatusData flightStatus;
146 * Set up the message
148 msg->sync = sync;
150 switch (osd_packet) {
151 case OSD_HK_PKT_TYPE_MISC:
152 break;
153 case OSD_HK_PKT_TYPE_NAV:
154 break;
155 case OSD_HK_PKT_TYPE_MAINT:
156 break;
157 case OSD_HK_PKT_TYPE_ATT:
158 msg->t = OSD_HK_PKT_TYPE_ATT;
159 float roll;
160 AttitudeStateRollGet(&roll);
161 blob->att.roll = (int16_t)(roll * 10);
163 float pitch;
164 AttitudeStatePitchGet(&pitch);
165 blob->att.pitch = (int16_t)(pitch * 10);
167 float yaw;
168 AttitudeStateYawGet(&yaw);
169 blob->att.yaw = (int16_t)(yaw * 10);
170 break;
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;
176 break;
177 default:
178 break;
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;
191 break;
192 case SYSTEMALARMS_ALARM_WARNING:
193 case SYSTEMALARMS_ALARM_ERROR:
194 case SYSTEMALARMS_ALARM_CRITICAL:
195 default:
196 blob->misc.low_battery = 1;
197 break;
200 newAlarmData = false;
203 #if FLIGHTBATTERYSUPPORTED
204 if (newBattData) {
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));
215 newBattData = false;
217 #else
218 // blob->misc.current = 0;
219 #endif
221 #if POSITIONACTUAL_SUPPORTED
222 if (newPositionStateData) {
223 PositionStateData position;
224 PositionStateGet(&position);
226 /* compute 3D distance */
227 float d = sqrt(
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 */
235 blob->misc.home = (
236 (home & 0xFF00 >> 8) |
237 (home & 0x00FF << 8));
239 newPositionStateData = false;
241 #else
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;
249 } else {
250 sync = OSD_HK_SYNC_A;
252 } else {
253 /* Failed to send this update */
254 osd_hk_msg_dropped++;
256 osd_packet++;
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);
269 return 0;
271 return -1;
274 static int32_t osdoutputInitialize(void)
276 osd_hk_com_id = PIOS_COM_OSDHK;
277 #ifdef MODULE_OSDOUTPUT_BUILTIN
278 osdoutputEnabled = 1;
279 #else
280 HwSettingsOptionalModulesData optionalModules;
281 HwSettingsOptionalModulesGet(&optionalModules);
282 if (optionalModules.OsdHk == HWSETTINGS_OPTIONALMODULES_ENABLED) {
283 osdoutputEnabled = 1;
284 } else {
285 osdoutputEnabled = 0;
287 #endif
289 if (osdoutputEnabled && osd_hk_com_id) {
290 PIOS_COM_ChangeBaud(osd_hk_com_id, 57600);
293 return 0;
295 MODULE_INITCALL(osdoutputInitialize, osdoutputStart);
298 * @}
299 * @}