flight/modules/Osd/osdoutput/osdoutput.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup OSDOUTPUTModule OSDOutput Module
   6  * @brief On screen display support
   7  * @{
   8  *
   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
  13  *
  14  *****************************************************************************/
  15 /*
  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.
  20  *
  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.
  25  *
  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
  29  */
  30
  31 #include "openpilot.h"
  32
  33 #if FLIGHTBATTERYSTATE_SUPPORTED
  34 #include "flightbatterystate.h"
  35 #endif
  36
  37 #if POSITIONACTUAL_SUPPORTED
  38 #include "positionstate.h"
  39 #endif
  40
  41 #include "systemalarms.h"
  42 #include "attitudestate.h"
  43 #include "hwsettings.h"
  44 #include "flightstatus.h"
  45
  46 static bool osdoutputEnabled;
  47
  48 enum osd_hk_sync {
  49     OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34,
  50 };
  51
  52 enum osd_hk_pkt_type {
  53     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,
  54 };
  55
  56 enum osd_hk_control_mode {
  57     OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2,
  58 };
  59
  60 struct osd_hk_blob_misc {
  61     uint8_t  type; /* Always OSD_HK_PKT_TYPE_MISC */
  62     int16_t  roll;
  63     int16_t  pitch;
  64     // uint16_t home;           /* Big Endian */
  65     enum osd_hk_control_mode control_mode;
  66     uint8_t  low_battery;
  67     uint16_t current; /* Big Endian */
  68 } __attribute__((packed));
  69
  70 struct osd_hk_blob_att {
  71     uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */
  72     int16_t roll;
  73     int16_t pitch;
  74     int16_t yaw;
  75     int16_t speed; /* Big Endian */
  76 } __attribute__((packed));
  77
  78 struct osd_hk_blob_nav {
  79     uint8_t  type; /* Always OSD_HK_PKT_TYPE_NAV */
  80     uint32_t gps_lat; /* Big Endian */
  81     uint32_t gps_lon; /* Big Endian */
  82 } __attribute__((packed));
  83
  84 struct osd_hk_blob_maint {
  85     uint8_t  type; /* Always OSD_HK_PKT_TYPE_MAINT */
  86     uint8_t  gps_speed;
  87     uint16_t gps_alt; /* Big Endian */
  88     uint16_t gps_dis; /* Big Endian */
  89     uint8_t  status;
  90     uint8_t  config;
  91     uint8_t  emerg;
  92 } __attribute__((packed));
  93
  94 struct osd_hk_blob_mode {
  95     uint8_t  type; /* Always OSD_HK_PKT_TYPE_MODE */
  96     uint8_t  fltmode;
  97     uint16_t gps_alt; /* Big Endian */
  98     uint16_t gps_dis; /* Big Endian */
  99     uint8_t  armed;
 100     uint8_t  config;
 101     uint8_t  emerg;
 102 } __attribute__((packed));
 103
 104 union osd_hk_pkt_blobs {
 105     struct osd_hk_blob_misc  misc;
 106     struct osd_hk_blob_nav   nav;
 107     struct osd_hk_blob_maint maint;
 108     struct osd_hk_blob_att   att;
 109     struct osd_hk_blob_mode  mode;
 110 } __attribute__((packed));
 111
 112 struct osd_hk_msg {
 113     enum osd_hk_sync       sync;
 114     enum osd_hk_pkt_type   t;
 115     union osd_hk_pkt_blobs v;
 116 } __attribute__((packed));
 117
 118 static struct osd_hk_msg osd_hk_msg_buf;
 119
 120 static volatile bool newPositionStateData = false;
 121 static volatile bool newBattData     = false;
 122 static volatile bool newAttitudeData = false;
 123 static volatile bool newAlarmData    = false;
 124
 125 static uint32_t osd_hk_com_id;
 126 static uint8_t osd_hk_msg_dropped;
 127 static uint8_t osd_packet;
 128
 129 static void send_update(__attribute__((unused)) UAVObjEvent *ev)
 130 {
 131     static enum osd_hk_sync sync = OSD_HK_SYNC_A;
 132
 133     struct osd_hk_msg *msg = &osd_hk_msg_buf;
 134     union osd_hk_pkt_blobs *blob = &(osd_hk_msg_buf.v);
 135
 136     /* Make sure we have a COM port bound */
 137     if (!osd_hk_com_id) {
 138         return;
 139     }
 140
 141     FlightStatusData flightStatus;
 142
 143     /*
 144      * Set up the message
 145      */
 146     msg->sync = sync;
 147
 148     switch (osd_packet) {
 149     case OSD_HK_PKT_TYPE_MISC:
 150         break;
 151     case OSD_HK_PKT_TYPE_NAV:
 152         break;
 153     case OSD_HK_PKT_TYPE_MAINT:
 154         break;
 155     case OSD_HK_PKT_TYPE_ATT:
 156         msg->t = OSD_HK_PKT_TYPE_ATT;
 157         float roll;
 158         AttitudeStateRollGet(&roll);
 159         blob->att.roll = (int16_t)(roll * 10);
 160
 161         float pitch;
 162         AttitudeStatePitchGet(&pitch);
 163         blob->att.pitch = (int16_t)(pitch * 10);
 164
 165         float yaw;
 166         AttitudeStateYawGet(&yaw);
 167         blob->att.yaw = (int16_t)(yaw * 10);
 168         break;
 169     case OSD_HK_PKT_TYPE_MODE:
 170         msg->t = OSD_HK_PKT_TYPE_MODE;
 171         FlightStatusGet(&flightStatus);
 172         blob->mode.fltmode = flightStatus.FlightMode;
 173         blob->mode.armed   = flightStatus.Armed;
 174         break;
 175     default:
 176         break;
 177     }
 178
 179     /* Field not supported yet */
 180 // blob->misc.control_mode = 0;
 181     /*if (newAlarmData) {
 182        SystemAlarmsData alarms;
 183        SystemAlarmsGet(&alarms);
 184
 185        switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) {
 186        case SYSTEMALARMS_ALARM_UNINITIALISED:
 187        case SYSTEMALARMS_ALARM_OK:
 188        blob->misc.low_battery = 0;
 189        break;
 190        case SYSTEMALARMS_ALARM_WARNING:
 191        case SYSTEMALARMS_ALARM_ERROR:
 192        case SYSTEMALARMS_ALARM_CRITICAL:
 193        default:
 194        blob->misc.low_battery = 1;
 195        break;
 196        }
 197
 198        newAlarmData = false;
 199        }*/
 200
 201 #if FLIGHTBATTERYSUPPORTED
 202     if (newBattData) {
 203         float consumed_energy;
 204         FlightBatteryStateConsumedEnergyGet(&consumed_energy);
 205
 206         uint16_t current = (uint16_t)(consumed_energy * 10);
 207
 208         /* convert to big endian */
 209         blob->misc.current = (
 210             (current & 0xFF00 >> 8) |
 211             (current & 0x00FF << 8));
 212
 213         newBattData = false;
 214     }
 215 #else
 216 // blob->misc.current = 0;
 217 #endif
 218
 219 #if POSITIONACTUAL_SUPPORTED
 220     if (newPositionStateData) {
 221         PositionStateData position;
 222         PositionStateGet(&position);
 223
 224         /* compute 3D distance */
 225         float d = sqrt(
 226             pow(position.North, 2) +
 227             pow(position.East, 2) +
 228             pow(position.Down, 2));
 229         /* convert from cm to dm (10ths of m) */
 230         uint16_t home = (uint16_t)(d / 10);
 231
 232         /* convert to big endian */
 233         blob->misc.home = (
 234             (home & 0xFF00 >> 8) |
 235             (home & 0x00FF << 8));
 236
 237         newPositionStateData = false;
 238     }
 239 #else
 240 // blob->misc.home = 0;
 241 #endif /* if POSITIONACTUAL_SUPPORTED */
 242
 243     if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *)&osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) {
 244         /* Sent a packet, flip to the opposite sync */
 245         if (sync == OSD_HK_SYNC_A) {
 246             sync = OSD_HK_SYNC_B;
 247         } else {
 248             sync = OSD_HK_SYNC_A;
 249         }
 250     } else {
 251         /* Failed to send this update */
 252         osd_hk_msg_dropped++;
 253     }
 254     osd_packet++;
 255     if (osd_packet > OSD_HK_PKT_TYPE_MODE) {
 256         osd_packet = OSD_HK_PKT_TYPE_MISC;
 257     }
 258 }
 259
 260 static UAVObjEvent ev;
 261
 262 static int32_t osdoutputStart(void)
 263 {
 264     if (osdoutputEnabled) {
 265         /* Start a periodic timer to kick sending of an update */
 266         EventPeriodicCallbackCreate(&ev, send_update, 25 / portTICK_RATE_MS);
 267         return 0;
 268     }
 269     return -1;
 270 }
 271
 272 static int32_t osdoutputInitialize(void)
 273 {
 274     osd_hk_com_id    = PIOS_COM_OSDHK;
 275 #ifdef MODULE_OSDOUTPUT_BUILTIN
 276     osdoutputEnabled = 1;
 277 #else
 278     HwSettingsInitialize();
 279     HwSettingsOptionalModulesData optionalModules;
 280     HwSettingsOptionalModulesGet(&optionalModules);
 281     if (optionalModules.OsdHk == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 282         osdoutputEnabled = 1;
 283     } else {
 284         osdoutputEnabled = 0;
 285     }
 286 #endif
 287     return 0;
 288 }
 289 MODULE_INITCALL(osdoutputInitialize, osdoutputStart);
 290
 291 /**
 292  * @}
 293  * @}
 294  */