diff --git a/libraries/AP_GPS/AP_GPS_PX4.cpp b/libraries/AP_GPS/AP_GPS_PX4.cpp new file mode 100644 index 0000000000000000000000000000000000000000..add1c92016421ece4c24c2c71cc9cc54f4b7ee3e --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_PX4.cpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +// GPS proxy driver for APM on PX4 platforms +// +// This driver subscribes on PX4s vehicle_gps_position topic and presents the data received to APM. +// The publisher could be an UAVCAN GNSS module like http://docs.zubax.com/Zubax_GNSS, or any other GNSS +// hardware supported by the PX4 ecosystem. +// +// Code by Holger Steinhaus + +#include "AP_GPS_PX4.h" +#include <uORB/uORB.h> + +#include <math.h> + +extern const AP_HAL::HAL& hal; + +AP_GPS_PX4::AP_GPS_PX4(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : + AP_GPS_Backend(_gps, _state, _port) +{ + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); +} + + +#define MS_PER_WEEK 7*24*3600*1000LL +#define DELTA_POSIX_GPS_EPOCH 315964800LL*1000LL +#define LEAP_MS_GPS_2014 16*1000LL + +// update internal state if new GPS information is available +bool +AP_GPS_PX4::read(void) +{ + bool updated = false; + orb_check(_gps_sub, &updated); + + if (updated) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos)) { + state.last_gps_time_ms = hal.scheduler->millis(); + state.status = (AP_GPS::GPS_Status) (_gps_pos.fix_type | AP_GPS::NO_FIX); + state.num_sats = _gps_pos.satellites_used; + state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f); + + state.location.lat = _gps_pos.lat; + state.location.lng = _gps_pos.lon; + state.location.alt = _gps_pos.alt/10; + + state.ground_speed = _gps_pos.vel_m_s; + state.ground_course_cd = int32_t(double(_gps_pos.cog_rad) / M_PI * 18000. +.5); + state.have_vertical_velocity = _gps_pos.vel_ned_valid; + state.velocity.x = _gps_pos.vel_n_m_s; + state.velocity.y = _gps_pos.vel_e_m_s; + state.velocity.z = _gps_pos.vel_d_m_s; + + // convert epoch timestamp back to gps epoch - evil hack until we get the genuine + // raw week information (or APM switches to Posix epoch ;-) ) + uint64_t epoch_ms = uint64_t(_gps_pos.time_gps_usec/1000.+.5); + uint64_t gps_ms = epoch_ms - DELTA_POSIX_GPS_EPOCH + LEAP_MS_GPS_2014; + state.time_week = uint16_t(gps_ms / uint64_t(MS_PER_WEEK)); + state.time_week_ms = uint32_t(gps_ms - uint64_t(state.time_week)*MS_PER_WEEK); + } + } + + return updated; +} \ No newline at end of file diff --git a/libraries/AP_GPS/AP_GPS_PX4.h b/libraries/AP_GPS/AP_GPS_PX4.h new file mode 100644 index 0000000000000000000000000000000000000000..6f5222cebd07663e18da3d020bda3c6273d01314 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_PX4.h @@ -0,0 +1,39 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +// +// GPS proxy driver for APM on PX4 platforms +// Code by Holger Steinhaus +// +#ifndef __AP_GPS_PX4_H__ +#define __AP_GPS_PX4_H__ + +#include <AP_HAL.h> +#include <AP_GPS.h> +#include <modules/uORB/topics/vehicle_gps_position.h> + +class AP_GPS_PX4 : public AP_GPS_Backend { +public: + AP_GPS_PX4(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + bool read(); + +private: + int _gps_sub; + struct vehicle_gps_position_s _gps_pos; +}; + +#endif // AP_GPS_SIRF_h