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