ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       Antennatrackwidget.cpp
   5  * @author     Sami Korhonen & the OpenPilot team Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup AntennaTrackGadgetPlugin Antenna Track Gadget Plugin
   9  * @{
  10  * @brief A gadget that communicates with antenna tracker and enables basic configuration
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 #include "antennatrackwidget.h"
  29 #include "extensionsystem/pluginmanager.h"
  30 #include "uavobjectmanager.h"
  31
  32
  33 #include <iostream>
  34 #include <QtGui>
  35 #include <QDebug>
  36
  37 /*
  38  * Initialize the widget
  39  */
  40 AntennaTrackWidget::AntennaTrackWidget(QWidget *parent) : QWidget(parent)
  41 {
  42     setupUi(this);
  43
  44     azimuth_old   = 0;
  45     elevation_old = 0;
  46 }
  47
  48 AntennaTrackWidget::~AntennaTrackWidget()
  49 {}
  50 void AntennaTrackWidget::setPort(QPointer<QSerialPort> portx)
  51 {
  52     port = portx;
  53 }
  54
  55 void AntennaTrackWidget::dumpPacket(const QString &packet)
  56 {
  57     textBrowser->append(packet);
  58     if (textBrowser->document()->lineCount() > 200) {
  59         QTextCursor tc = textBrowser->textCursor();
  60         tc.movePosition(QTextCursor::Start);
  61         tc.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor);
  62         tc.movePosition(QTextCursor::StartOfLine, QTextCursor::KeepAnchor);
  63         tc.removeSelectedText();
  64     }
  65 }
  66
  67 void AntennaTrackWidget::setPosition(double lat, double lon, double alt)
  68 {
  69     // lat *= 1E-7;
  70     // lon *= 1E-7;
  71     double deg = floor(fabs(lat));
  72     double min = (fabs(lat) - deg) * 60;
  73     QString str1;
  74
  75     str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min);
  76     if (lat > 0) {
  77         str1.append("N");
  78     } else {
  79         str1.append("S");
  80     }
  81     coord_value->setText(str1);
  82     deg = floor(fabs(lon));
  83     min = (fabs(lon) - deg) * 60;
  84     QString str2;
  85     str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min);
  86     if (lon > 0) {
  87         str2.append("E");
  88     } else {
  89         str2.append("W");
  90     }
  91     coord_value_2->setText(str2);
  92     QString str3;
  93     str3.sprintf("%.2f m", alt);
  94     coord_value_3->setText(str3);
  95     TrackData.Latitude  = lat;
  96     TrackData.Longitude = lon;
  97     TrackData.Altitude  = alt;
  98     calcAntennaPosition();
  99 }
 100
 101 void AntennaTrackWidget::setHomePosition(double lat, double lon, double alt)
 102 {
 103     // lat *= 1E-7;
 104     // lon *= 1E-7;
 105     double deg = floor(fabs(lat));
 106     double min = (fabs(lat) - deg) * 60;
 107     QString str1;
 108
 109     str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min);
 110     if (lat > 0) {
 111         str1.append("N");
 112     } else {
 113         str1.append("S");
 114     }
 115     speed_value->setText(str1);
 116     deg = floor(fabs(lon));
 117     min = (fabs(lon) - deg) * 60;
 118     QString str2;
 119     str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min);
 120     if (lon > 0) {
 121         str2.append("E");
 122     } else {
 123         str2.append("W");
 124     }
 125     bear_label->setText(str2);
 126     QString str3;
 127     str3.sprintf("%.2f m", alt);
 128     bear_value->setText(str3);
 129     TrackData.HomeLatitude  = lat;
 130     TrackData.HomeLongitude = lon;
 131     TrackData.HomeAltitude  = alt;
 132     calcAntennaPosition();
 133 }
 134
 135 void AntennaTrackWidget::calcAntennaPosition(void)
 136 {
 137     /** http://www.movable-type.co.uk/scripts/latlong.html **/
 138     double lat1, lat2, lon1, lon2, a, c, d, x, y, brng;
 139     double azimuth, elevation;
 140     double gcsAlt = TrackData.HomeAltitude; // Home MSL altitude
 141     double uavAlt = TrackData.Altitude; // UAV MSL altitude
 142     double dAlt   = uavAlt - gcsAlt; // Altitude difference
 143
 144     // Convert to radians
 145     lat1 = TrackData.HomeLatitude * (M_PI / 180); // Home lat
 146     lon1 = TrackData.HomeLongitude * (M_PI / 180); // Home lon
 147     lat2 = TrackData.Latitude * (M_PI / 180); // UAV lat
 148     lon2 = TrackData.Longitude * (M_PI / 180); // UAV lon
 149
 150     // Bearing
 151     /**
 152        var y = Math.sin(dLon) * Math.cos(lat2);
 153        var x = Math.cos(lat1)*Math.sin(lat2) -
 154             Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
 155        var brng = Math.atan2(y, x).toDeg();
 156      **/
 157     y    = sin(lon2 - lon1) * cos(lat2);
 158     x    = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1);
 159     brng = atan2((sin(lon2 - lon1) * cos(lat2)), (cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1))) * (180 / M_PI);
 160     if (brng < 0) {
 161         brng += 360;
 162     }
 163
 164     // bearing to stepper
 165     azimuth = brng;
 166
 167     // Haversine formula for distance
 168     /**
 169        var R = 6371; // km
 170        var dLat = (lat2-lat1).toRad();
 171        var dLon = (lon2-lon1).toRad();
 172        var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
 173             Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
 174             Math.sin(dLon/2) * Math.sin(dLon/2);
 175        var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
 176        var d = R * c;
 177      **/
 178     a = sin((lat2 - lat1) / 2) * sin((lat2 - lat1) / 2) +
 179         cos(lat1) * cos(lat2) *
 180         sin((lon2 - lon1) / 2) * sin((lon2 - lon1) / 2);
 181     c = 2 * atan2(sqrt(a), sqrt(1 - a));
 182     d = 6371 * 1000 * c;
 183
 184     // Elevation  v depends servo direction
 185     if (d != 0) {
 186         elevation = 90 - (atan(dAlt / d) * (180 / M_PI));
 187     } else {
 188         elevation = 0;
 189     }
 190     // ! TODO: sanity check
 191
 192     QString str3;
 193     str3.sprintf("%.0f deg", azimuth);
 194     azimuth_value->setText(str3);
 195
 196     str3.sprintf("%.0f deg", elevation);
 197     elevation_value->setText(str3);
 198
 199     // servo value 2000-4000
 200     int servo   = (int)(2000.0 / 180 * elevation + 2000);
 201     int stepper = (int)(400.0 / 360 * (azimuth - azimuth_old));
 202
 203     // send azimuth and elevation to tracker hardware
 204     str3.sprintf("move %d 2000 2000 2000 %d\r", stepper, servo);
 205     if (port->isOpen()) {
 206         if (azimuth_old != azimuth || elevation != elevation_old) {
 207             port->write(str3.toLatin1());
 208         }
 209     }
 210     azimuth_old   = azimuth;
 211     elevation_old = elevation;
 212 }