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             : #include "AP_GPS_SITL.h"
      17             : 
      18             : #if AP_SIM_GPS_ENABLED
      19             : 
      20             : #include <ctype.h>
      21             : #include <stdint.h>
      22             : #include <stdlib.h>
      23             : #include <stdio.h>
      24             : #include <sys/time.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : /*
      29             :   return GPS time of week in milliseconds
      30             :  */
      31             : /*
      32             :   get timeval using simulation time
      33             :  */
      34         146 : static void simulation_timeval(struct timeval *tv)
      35             : {
      36         146 :     uint64_t now = AP_HAL::micros64();
      37             :     static uint64_t first_usec;
      38             :     static struct timeval first_tv;
      39         146 :     if (first_usec == 0) {
      40           1 :         first_usec = now;
      41           1 :         first_tv.tv_sec = AP::sitl()->start_time_UTC;
      42             :     }
      43         146 :     *tv = first_tv;
      44         146 :     tv->tv_sec += now / 1000000ULL;
      45         146 :     uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
      46         146 :     tv->tv_sec += new_usec / 1000000ULL;
      47         146 :     tv->tv_usec = new_usec % 1000000ULL;
      48         146 : }
      49         146 : static void gps_time(uint16_t *time_week, uint32_t *time_week_ms)
      50             : {
      51             :     struct timeval tv;
      52         146 :     simulation_timeval(&tv);
      53         146 :     const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - (GPS_LEAPSECONDS_MILLIS / 1000ULL);
      54         146 :     uint32_t epoch_seconds = tv.tv_sec - epoch;
      55         146 :     *time_week = epoch_seconds / AP_SEC_PER_WEEK;
      56         146 :     uint32_t t_ms = tv.tv_usec / 1000;
      57             :     // round time to nearest 200ms
      58         146 :     *time_week_ms = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200);
      59         146 : }
      60             : 
      61        1467 : bool AP_GPS_SITL::read(void)
      62             : {
      63        1467 :     const uint32_t now = AP_HAL::millis();
      64        1467 :     if (now - last_update_ms < 200) {
      65        1321 :         return false;
      66             :     }
      67         146 :     last_update_ms = now;
      68             : 
      69         146 :     auto *sitl = AP::sitl();
      70             : 
      71         146 :     double latitude =sitl->state.latitude;
      72         146 :     double longitude = sitl->state.longitude;
      73         146 :     float altitude = sitl->state.altitude;
      74         146 :     const double speedN = sitl->state.speedN;
      75         146 :     const double speedE = sitl->state.speedE;
      76         146 :     const double speedD = sitl->state.speedD;
      77             :     // const double yaw = sitl->state.yawDeg;
      78             : 
      79             :     uint16_t time_week;
      80             :     uint32_t time_week_ms;
      81             : 
      82         146 :     gps_time(&time_week, &time_week_ms);
      83             : 
      84         146 :     state.time_week = time_week;
      85         146 :     state.time_week_ms = time_week_ms;
      86         146 :     state.status = AP_GPS_FixType::FIX_3D;
      87         146 :     state.num_sats = 15;
      88             : 
      89         292 :     state.location = Location{
      90         146 :         int32_t(latitude*1e7),
      91         146 :         int32_t(longitude*1e7),
      92         146 :         int32_t(altitude*100),
      93             :         Location::AltFrame::ABSOLUTE
      94             :     };
      95             : 
      96         146 :     state.hdop = 100;
      97         146 :     state.vdop = 100;
      98             : 
      99         146 :     state.have_vertical_velocity = true;
     100         146 :     state.velocity.x = speedN;
     101         146 :     state.velocity.y = speedE;
     102         146 :     state.velocity.z = speedD;
     103             : 
     104         146 :     velocity_to_speed_course(state);
     105             : 
     106         146 :     state.have_speed_accuracy = true;
     107         146 :     state.have_horizontal_accuracy = true;
     108         146 :     state.have_vertical_accuracy = true;
     109         146 :     state.have_vertical_velocity = true;
     110             : 
     111             :     // state.horizontal_accuracy = pkt.horizontal_pos_accuracy;
     112             :     // state.vertical_accuracy = pkt.vertical_pos_accuracy;
     113             :     // state.speed_accuracy = pkt.horizontal_vel_accuracy;
     114             : 
     115         146 :     state.last_gps_time_ms = now;
     116             : 
     117         146 :     return true;
     118             : }
     119             : 
     120             : #endif  // AP_SIM_GPS_ENABLED