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

70
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) static void NOINLINE send_location(mavlink_channel_t chan)
{ {
uint32_t fix_time; uint32_t fix_time;
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) { if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
fix_time = g_gps->last_fix_time; fix_time = gps.last_fix_time_ms();
} else { } else {
fix_time = hal.scheduler->millis(); fix_time = hal.scheduler->millis();
} }
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send( mavlink_msg_global_position_int_send(
chan, chan,
fix_time, fix_time,
@ -96,31 +97,58 @@ static void NOINLINE send_location(mavlink_channel_t chan)
current_loc.lng, // in 1E7 degrees current_loc.lng, // in 1E7 degrees
current_loc.alt * 10, // millimeters above sea level current_loc.alt * 10, // millimeters above sea level
0, 0,
g_gps->velocity_north() * 100, // X speed cm/s (+ve North) vel.x * 100, // X speed cm/s (+ve North)
g_gps->velocity_east() * 100, // Y speed cm/s (+ve East) vel.y * 100, // Y speed cm/s (+ve East)
g_gps->velocity_down() * -100, // Z speed cm/s (+ve up) vel.z * -100, // Z speed cm/s (+ve up)
ahrs.yaw_sensor); ahrs.yaw_sensor);
} }
static void NOINLINE send_gps_raw(mavlink_channel_t chan) static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{ {
static uint32_t last_send_time; static uint32_t last_send_time_ms;
if (last_send_time != 0 && last_send_time == g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_3D) { if (last_send_time_ms == 0 || last_send_time_ms != gps.last_message_time_ms(0)) {
return; last_send_time_ms = gps.last_message_time_ms(0);
const Location &loc = gps.location(0);
mavlink_msg_gps_raw_int_send(
chan,
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,
gps.ground_speed(0)*100, // cm/s
gps.ground_course_cd(0), // 1/100 degrees,
gps.num_sats(0));
} }
last_send_time = g_gps->last_fix_time;
mavlink_msg_gps_raw_int_send( #if HAL_CPU_CLASS > HAL_CPU_CLASS_16
chan, static uint32_t last_send_time_ms2;
g_gps->last_fix_time*(uint64_t)1000, if (gps.num_sensors() > 1 &&
g_gps->status(), gps.status(1) > AP_GPS::NO_GPS &&
g_gps->latitude, // in 1E7 degrees (last_send_time_ms2 == 0 || last_send_time_ms2 != gps.last_message_time_ms(1))) {
g_gps->longitude, // in 1E7 degrees int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
g_gps->altitude_cm * 10, // in mm if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
g_gps->hdop, const Location &loc = gps.location(1);
65535, last_send_time_ms = gps.last_message_time_ms(1);
g_gps->ground_speed_cm, // cm/s mavlink_msg_gps2_raw_send(
g_gps->ground_course_cd, // 1/100 degrees, chan,
g_gps->num_sats); 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) 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) static void update_GPS(void)
{ {
g_gps->update(); gps.update();
static uint32_t last_gps_msg_ms;
static uint8_t ground_start_count = 5; static uint8_t ground_start_count = 5;
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) { if (gps.last_message_time_ms() != last_gps_msg_ms &&
g_gps->new_data = false; gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
if(ground_start_count > 1) { if(ground_start_count > 1) {
ground_start_count--; ground_start_count--;
@ -78,18 +80,15 @@ static void update_GPS(void)
} else { } else {
// Now have an initial GPS position // Now have an initial GPS position
// use it as the HOME position in future startups // use it as the HOME position in future startups
current_loc.lat = g_gps->latitude; current_loc = gps.location();
current_loc.lng = g_gps->longitude;
current_loc.alt = g_gps->altitude_cm;
current_loc.options = 0; // Absolute altitude
set_home(current_loc); set_home(current_loc);
// set system clock for log timestamps // 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) { if (g.compass_enabled) {
// Set compass declination automatically // 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; 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 // GPS Initialization
g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY, NULL); gps.init(NULL);
mavlink_system.compid = 4; mavlink_system.compid = 4;
mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER; 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 // 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? // REVISIT: what if we lose lock during a mission and the antenna is moving?
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) { if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
current_loc.lat = g_gps->latitude; current_loc = gps.location();
current_loc.lng = g_gps->longitude;
current_loc.alt = g_gps->altitude_cm;
current_loc.options = 0; // Absolute altitude
} }
// calculate the bearing to the vehicle // calculate the bearing to the vehicle