Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: convert to new GPS API

This commit is contained in:
Andrew Tridgell 2014-03-31 18:32:14 +11:00
parent bc1ebcadf5
commit c51212da53
5 changed files with 63 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -119,8 +119,7 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...);
////////////////////////////////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
// All GPS access should be through this pointer.
static GPS *g_gps;
static AP_GPS gps;
#if CONFIG_BARO == AP_BARO_BMP085
static AP_Baro_BMP085 barometer;
@ -150,9 +149,6 @@ static AP_Compass_HIL compass;
#error Unrecognized CONFIG_COMPASS setting
#endif
// GPS selection
AP_GPS_Auto g_gps_driver(&g_gps);
#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
AP_InertialSensor_MPU6000 ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
@ -171,9 +167,9 @@ AP_InertialSensor_L3G4200D ins;
// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
AP_AHRS_NavEKF ahrs(ins, barometer, gps);
#else
AP_AHRS_DCM ahrs(ins, barometer, g_gps);
AP_AHRS_DCM ahrs(ins, barometer, gps);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

66
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -84,11 +84,12 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
static void NOINLINE send_location(mavlink_channel_t chan)
{
uint32_t fix_time;
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
fix_time = g_gps->last_fix_time;
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
fix_time = gps.last_fix_time_ms();
} else {
fix_time = hal.scheduler->millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
chan,
fix_time,
@ -96,31 +97,58 @@ static void NOINLINE send_location(mavlink_channel_t chan)
current_loc.lng, // in 1E7 degrees
current_loc.alt * 10, // millimeters above sea level
0,
g_gps->velocity_north() * 100, // X speed cm/s (+ve North)
g_gps->velocity_east() * 100, // Y speed cm/s (+ve East)
g_gps->velocity_down() * -100, // Z speed cm/s (+ve up)
vel.x * 100, // X speed cm/s (+ve North)
vel.y * 100, // Y speed cm/s (+ve East)
vel.z * -100, // Z speed cm/s (+ve up)
ahrs.yaw_sensor);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time;
if (last_send_time != 0 && last_send_time == g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
return;
}
last_send_time = g_gps->last_fix_time;
static uint32_t last_send_time_ms;
if (last_send_time_ms == 0 || last_send_time_ms != gps.last_message_time_ms(0)) {
last_send_time_ms = gps.last_message_time_ms(0);
const Location &loc = gps.location(0);
mavlink_msg_gps_raw_int_send(
chan,
g_gps->last_fix_time*(uint64_t)1000,
g_gps->status(),
g_gps->latitude, // in 1E7 degrees
g_gps->longitude, // in 1E7 degrees
g_gps->altitude_cm * 10, // in mm
g_gps->hdop,
gps.last_fix_time_ms(0)*(uint64_t)1000,
gps.status(0),
loc.lat, // in 1E7 degrees
loc.lng, // in 1E7 degrees
loc.alt * 10UL, // in mm
gps.get_hdop(0),
65535,
g_gps->ground_speed_cm, // cm/s
g_gps->ground_course_cd, // 1/100 degrees,
g_gps->num_sats);
gps.ground_speed(0)*100, // cm/s
gps.ground_course_cd(0), // 1/100 degrees,
gps.num_sats(0));
}
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
static uint32_t last_send_time_ms2;
if (gps.num_sensors() > 1 &&
gps.status(1) > AP_GPS::NO_GPS &&
(last_send_time_ms2 == 0 || last_send_time_ms2 != gps.last_message_time_ms(1))) {
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
const Location &loc = gps.location(1);
last_send_time_ms = gps.last_message_time_ms(1);
mavlink_msg_gps2_raw_send(
chan,
gps.last_fix_time_ms(1)*(uint64_t)1000,
gps.status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
gps.get_hdop(1),
65535,
gps.ground_speed(1)*100, // cm/s
gps.ground_course_cd(1), // 1/100 degrees,
gps.num_sats(1),
0,
0);
}
}
#endif
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)

17
Tools/AntennaTracker/sensors.pde
View File

@ -60,11 +60,13 @@ static void barometer_accumulate(void)
*/
static void update_GPS(void)
{
g_gps->update();
gps.update();
static uint32_t last_gps_msg_ms;
static uint8_t ground_start_count = 5;
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
g_gps->new_data = false;
if (gps.last_message_time_ms() != last_gps_msg_ms &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
if(ground_start_count > 1) {
ground_start_count--;
@ -78,18 +80,15 @@ static void update_GPS(void)
} else {
// Now have an initial GPS position
// use it as the HOME position in future startups
current_loc.lat = g_gps->latitude;
current_loc.lng = g_gps->longitude;
current_loc.alt = g_gps->altitude_cm;
current_loc.options = 0; // Absolute altitude
current_loc = gps.location();
set_home(current_loc);
// set system clock for log timestamps
hal.util->set_system_clock(g_gps->time_epoch_usec());
hal.util->set_system_clock(gps.time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
ground_start_count = 0;
}

5
Tools/AntennaTracker/system.pde
View File

@ -50,11 +50,8 @@ static void init_tracker()
}
}
// Do GPS init
g_gps = &g_gps_driver;
// GPS Initialization
g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY, NULL);
gps.init(NULL);
mavlink_system.compid = 4;
mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER;

7
Tools/AntennaTracker/tracking.pde
View File

@ -220,11 +220,8 @@ static void update_tracking(void)
// update our position if we have at least a 2D fix
// REVISIT: what if we lose lock during a mission and the antenna is moving?
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
current_loc.lat = g_gps->latitude;
current_loc.lng = g_gps->longitude;
current_loc.alt = g_gps->altitude_cm;
current_loc.options = 0; // Absolute altitude
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
current_loc = gps.location();
}
// calculate the bearing to the vehicle