flight/modules/Osd/osdinput/osdinput.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup osdinputModule osdinput Module
   6  * @brief Process osdinput information
   7  * @{
   8  *
   9  * @file       osdinput.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      osdinput module, handles osdinput stream
  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 // ****************
  32
  33 #include <openpilot.h>
  34
  35 #include "osdinput.h"
  36
  37 #include "attitudestate.h"
  38 #include "taskinfo.h"
  39 #include "flightstatus.h"
  40
  41 #include "fifo_buffer.h"
  42
  43 // ****************
  44 // Private functions
  45
  46 static void osdinputTask(void *parameters);
  47
  48 // ****************
  49 // Private constants
  50
  51 #define STACK_SIZE_BYTES  1024
  52 #define TASK_PRIORITY     (tskIDLE_PRIORITY + 4)
  53 #define MAX_PACKET_LENGTH 33
  54 // ****************
  55 // Private variables
  56
  57 static uint32_t oposdPort;
  58
  59 static xTaskHandle osdinputTaskHandle;
  60
  61 static char *oposd_rx_buffer;
  62 t_fifo_buffer rx;
  63
  64 enum osd_pkt_type {
  65     OSD_PKT_TYPE_MISC = 0, OSD_PKT_TYPE_NAV = 1, OSD_PKT_TYPE_MAINT = 2, OSD_PKT_TYPE_ATT = 3, OSD_PKT_TYPE_MODE = 4,
  66 };
  67
  68 // ****************
  69 /**
  70  * Initialise the osdinput module
  71  * \return -1 if initialisation failed
  72  * \return 0 on success
  73  */
  74
  75 int32_t osdinputStart(void)
  76 {
  77     // Start osdinput task
  78     xTaskCreate(osdinputTask, "OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle);
  79
  80     return 0;
  81 }
  82 /**
  83  * Initialise the gps module
  84  * \return -1 if initialisation failed
  85  * \return 0 on success
  86  */
  87 int32_t osdinputInitialize(void)
  88 {
  89     AttitudeStateInitialize();
  90     FlightStatusInitialize();
  91     // Initialize quaternion
  92     AttitudeStateData attitude;
  93     AttitudeStateGet(&attitude);
  94     attitude.q1    = 1;
  95     attitude.q2    = 0;
  96     attitude.q3    = 0;
  97     attitude.q4    = 0;
  98     attitude.Roll  = 0;
  99     attitude.Pitch = 0;
 100     attitude.Yaw   = 0;
 101     AttitudeStateSet(&attitude);
 102
 103     oposdPort = PIOS_COM_OSD;
 104
 105     oposd_rx_buffer = pios_malloc(MAX_PACKET_LENGTH);
 106     PIOS_Assert(oposd_rx_buffer);
 107
 108     return 0;
 109 }
 110 MODULE_INITCALL(osdinputInitialize, osdinputStart);
 111
 112 // ****************
 113 /**
 114  * Main osdinput task. It does not return.
 115  */
 116
 117 static void osdinputTask(__attribute__((unused)) void *parameters)
 118 {
 119     portTickType xDelay = 100 / portTICK_RATE_MS;
 120     portTickType lastSysTime;
 121
 122     lastSysTime = xTaskGetTickCount();
 123
 124     uint8_t rx_count = 0;
 125     bool start_flag  = false;
 126     int32_t osdRxOverflow = 0;
 127     uint8_t c = 0xAA;
 128     // Loop forever
 129     while (1) {
 130         // This blocks the task until there is something on the buffer
 131         while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) {
 132             // detect start while acquiring stream
 133             if (!start_flag && ((c == 0xCB) || (c == 0x34))) {
 134                 start_flag = true;
 135                 rx_count   = 0;
 136             } else if (!start_flag) {
 137                 continue;
 138             }
 139
 140             if (rx_count >= 11) {
 141                 // Flush the buffer and note the overflow event.
 142                 osdRxOverflow++;
 143                 start_flag = false;
 144                 rx_count   = 0;
 145             } else {
 146                 oposd_rx_buffer[rx_count] = c;
 147                 rx_count++;
 148             }
 149             if (rx_count == 11) {
 150                 if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) {
 151                     AttitudeStateData attitude;
 152                     AttitudeStateGet(&attitude);
 153                     attitude.q1    = 1;
 154                     attitude.q2    = 0;
 155                     attitude.q3    = 0;
 156                     attitude.q4    = 0;
 157                     attitude.Roll  = (float)((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f;
 158                     attitude.Pitch = (float)((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f;
 159                     attitude.Yaw   = (float)((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f;
 160                     AttitudeStateSet(&attitude);
 161                 } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) {
 162                     FlightStatusData status;
 163                     FlightStatusGet(&status);
 164                     status.Armed = oposd_rx_buffer[8];
 165                     status.FlightMode = oposd_rx_buffer[3];
 166                     FlightStatusSet(&status);
 167                 }
 168                 // frame completed
 169                 start_flag = false;
 170                 rx_count   = 0;
 171             }
 172         }
 173         vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
 174     }
 175 }
 176
 177 // ****************
 178
 179 /**
 180  * @}
 181  * @}
 182  */