Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : //
      17             : //  MAVLINK GPS driver
      18             : //
      19             : 
      20             : #include "AP_GPS_config.h"
      21             : 
      22             : #if AP_GPS_MAV_ENABLED
      23             : 
      24             : #include "AP_GPS_MAV.h"
      25             : #include <stdint.h>
      26             : 
      27             : // Reading does nothing in this class; we simply return whether or not
      28             : // the latest reading has been consumed.  By calling this function we assume
      29             : // the caller is consuming the new data;
      30        5456 : bool AP_GPS_MAV::read(void)
      31             : {
      32        5456 :     if (_new_data) {
      33         914 :         _new_data = false;
      34         914 :         return true;
      35             :     }
      36             : 
      37        4542 :     return false;
      38             : }
      39             : 
      40             : // handles an incoming mavlink message (HIL_GPS) and sets
      41             : // corresponding gps data appropriately;
      42         915 : void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
      43             : {
      44         915 :     switch (msg.msgid) {
      45             : 
      46         915 :         case MAVLINK_MSG_ID_GPS_INPUT: {
      47             :             mavlink_gps_input_t packet;
      48         915 :             mavlink_msg_gps_input_decode(&msg, &packet);
      49             : 
      50             :             // check if target instance belongs to incoming gps data.
      51         915 :             if (state.instance != packet.gps_id) {
      52           0 :                 return;
      53             :             }
      54             : 
      55         915 :             bool have_alt    = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_ALT) == 0);
      56         915 :             bool have_hdop   = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HDOP) == 0);
      57         915 :             bool have_vdop   = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VDOP) == 0);
      58         915 :             bool have_vel_h  = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_HORIZ) == 0);
      59         915 :             bool have_vel_v  = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_VERT) == 0);
      60         915 :             bool have_sa     = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY) == 0);
      61         915 :             bool have_ha     = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY) == 0);
      62         915 :             bool have_va     = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY) == 0);
      63         915 :             bool have_yaw    = (packet.yaw != 0);
      64             : 
      65         915 :             state.time_week     = packet.time_week;
      66         915 :             state.time_week_ms  = packet.time_week_ms;
      67         915 :             state.status = (AP_GPS_FixType)packet.fix_type;
      68             : 
      69         915 :             Location loc = {};
      70         915 :             loc.lat = packet.lat;
      71         915 :             loc.lng = packet.lon;
      72         915 :             if (have_alt) {
      73         915 :                 loc.alt = packet.alt * 100; // convert to centimeters
      74             :             }
      75         915 :             state.location = loc;
      76             : 
      77         915 :             if (have_hdop) {
      78         915 :                 state.hdop = packet.hdop * 100; // convert to centimeters
      79             :             }
      80             : 
      81         915 :             if (have_vdop) {
      82         915 :                 state.vdop = packet.vdop * 100; // convert to centimeters
      83             :             }
      84             : 
      85         915 :             if (have_vel_h) {
      86         915 :                 Vector3f vel(packet.vn, packet.ve, 0);
      87         915 :                 if (have_vel_v) {
      88         915 :                     vel.z = packet.vd;
      89         915 :                     state.have_vertical_velocity = true;
      90             :                 }
      91             : 
      92         915 :                 state.velocity = vel;
      93         915 :                 velocity_to_speed_course(state);
      94             :             }
      95             : 
      96         915 :             if (have_sa) {
      97         915 :                 state.speed_accuracy = packet.speed_accuracy;
      98         915 :                 state.have_speed_accuracy = true;
      99             :             }
     100             : 
     101         915 :             if (have_ha) {
     102         915 :                 state.horizontal_accuracy = packet.horiz_accuracy;
     103         915 :                 state.have_horizontal_accuracy = true;
     104             :             }
     105             : 
     106         915 :             if (have_va) {
     107         915 :                 state.vertical_accuracy = packet.vert_accuracy;
     108         915 :                 state.have_vertical_accuracy = true;
     109             :             }
     110             : 
     111         915 :             const uint32_t now_ms = AP_HAL::millis();
     112             : 
     113         915 :             if (have_yaw) {
     114           0 :                 state.gps_yaw = wrap_360(packet.yaw*0.01);
     115           0 :                 state.gps_yaw_time_ms = now_ms;
     116           0 :                 state.have_gps_yaw = true;
     117           0 :                 state.gps_yaw_configured = true;
     118             :             }
     119             : 
     120         915 :             if (packet.fix_type >= 3 && packet.time_week > 0) {
     121             :                 /*
     122             :                   use the millisecond timestamp from the GPS_INPUT
     123             :                   packet into jitter correction to get a local
     124             :                   timestamp corrected for transport jitter
     125             :                 */
     126         915 :                 if (first_week == 0) {
     127           1 :                     first_week = packet.time_week;
     128             :                 }
     129         915 :                 uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms;
     130         915 :                 uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms);
     131         915 :                 state.last_corrected_gps_time_us = (corrected_ms * 1000ULL);
     132         915 :                 state.corrected_timestamp_updated = true;
     133         915 :                 if (state.last_corrected_gps_time_us) {
     134         915 :                     _last_itow_ms = state.time_week_ms;
     135         915 :                     _have_itow = true;
     136             :                 }
     137         915 :                 if (have_yaw) {
     138           0 :                     state.gps_yaw_time_ms = corrected_ms;
     139             :                 }
     140             :             }
     141             : 
     142         915 :             state.num_sats = packet.satellites_visible;
     143         915 :             state.last_gps_time_ms = now_ms;
     144         915 :             _new_data = true;
     145         915 :             break;
     146             :         }
     147             : 
     148           0 :         default:
     149             :             // ignore all other messages
     150           0 :             break;
     151             :     }
     152             : }
     153             : #endif