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

Plane: convert to new GPS driver API

This commit is contained in:
Andrew Tridgell 2014-03-29 06:52:48 +11:00
parent d04d33a02d
commit e19341ca32
13 changed files with 119 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
ArduPlane/ArduPlane.pde
View File

@ -190,11 +190,8 @@ static int32_t pitch_limit_min_cd;
// supply data from the simulation.
//
// All GPS access should be through this pointer.
static GPS *g_gps;
#if GPS2_ENABLE
static GPS *g_gps2;
#endif
// GPS driver
static AP_GPS gps;
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
@ -227,38 +224,6 @@ static AP_Compass_HIL compass;
#error Unrecognized CONFIG_COMPASS setting
#endif
// GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&g_gps);
#if GPS2_ENABLE
AP_GPS_Auto g_gps2_driver(&g_gps2);
#endif
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19
AP_GPS_MTK19 g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_HIL
AP_GPS_HIL g_gps_driver;
#else
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#if CONFIG_INS_TYPE == CONFIG_INS_OILPAN || CONFIG_HAL_BOARD == HAL_BOARD_APM1
AP_ADC_ADS7844 apm1_adc;
#endif
@ -281,9 +246,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
static AP_L1_Control L1_controller(ahrs);
@ -690,13 +655,13 @@ static uint16_t mainLoop_count;
#if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
static AP_Mount camera_mount(&current_loc, g_gps, ahrs, 0);
static AP_Mount camera_mount(&current_loc, ahrs, 0);
#endif
#if MOUNT2 == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
static AP_Mount camera_mount2(&current_loc, g_gps, ahrs, 1);
static AP_Mount camera_mount2(&current_loc, ahrs, 1);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -1003,13 +968,13 @@ static void compass_save()
static void airspeed_ratio_update(void)
{
if (!airspeed.enabled() ||
g_gps->status() < GPS::GPS_OK_FIX_3D ||
g_gps->ground_speed_cm < 400) {
gps.status() < AP_GPS::GPS_OK_FIX_3D ||
gps.ground_speed() < 4) {
// don't calibrate when not moving
return;
}
if (airspeed.get_airspeed() < aparm.airspeed_min &&
g_gps->ground_speed_cm < (uint32_t)aparm.airspeed_min*100) {
gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
// don't calibrate when flying below the minimum airspeed. We
// check both airspeed and ground speed to catch cases where
// the airspeed ratio is way too low, which could lead to it
@ -1022,7 +987,7 @@ static void airspeed_ratio_update(void)
// don't calibrate when going beyond normal flight envelope
return;
}
Vector3f vg = g_gps->velocity_vector();
const Vector3f &vg = gps.velocity();
airspeed.update_calibration(vg);
gcs_send_airspeed_calibration(vg);
}
@ -1034,27 +999,14 @@ static void airspeed_ratio_update(void)
static void update_GPS_50Hz(void)
{
static uint32_t last_gps_reading;
g_gps->update();
gps.update();
if (g_gps->last_message_time_ms() != last_gps_reading) {
last_gps_reading = g_gps->last_message_time_ms();
if (gps.last_message_time_ms() != last_gps_reading) {
last_gps_reading = gps.last_message_time_ms();
if (should_log(MASK_LOG_GPS)) {
Log_Write_GPS();
}
}
#if GPS2_ENABLE
static uint32_t last_gps2_reading;
if (g_gps2 != NULL) {
g_gps2->update();
if (g_gps2->last_message_time_ms() != last_gps2_reading) {
last_gps2_reading = g_gps2->last_message_time_ms();
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS2(g_gps2);
}
}
}
#endif
}
/*
@ -1065,10 +1017,11 @@ static void update_GPS_10Hz(void)
// get position from AHRS
have_position = ahrs.get_position(current_loc);
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
g_gps->new_data = false;
static uint32_t last_gps_msg_ms;
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) {
if (ground_start_count > 1) {
ground_start_count--;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
@ -1081,11 +1034,12 @@ static void update_GPS_10Hz(void)
init_home();
// 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);
const Location &loc = gps.location();
compass.set_initial_location(loc.lat, loc.lng);
}
ground_start_count = 0;
}
@ -1143,7 +1097,7 @@ static void handle_auto_mode(void)
if (nav_pitch_cd < takeoff_pitch_cd)
nav_pitch_cd = takeoff_pitch_cd;
} else {
nav_pitch_cd = (g_gps->ground_speed_cm / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd;
nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd;
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
}

20
ArduPlane/Attitude.pde
View File

@ -345,7 +345,7 @@ static void stabilize()
if (channel_throttle->control_in == 0 &&
relative_altitude_abs_cm() < 500 &&
fabs(barometer.get_climb_rate()) < 0.5f &&
g_gps->ground_speed_cm < 300) {
gps.ground_speed() < 3) {
// we are low, with no climb rate, and zero throttle, and very
// low ground speed. Zero the attitude controller
// integrators. This prevents integrator buildup pre-takeoff.
@ -410,7 +410,7 @@ static void calc_nav_yaw_course(void)
*/
static void calc_nav_yaw_ground(void)
{
if (g_gps->ground_speed_cm < 100 &&
if (gps.ground_speed() < 1 &&
channel_throttle->control_in == 0) {
// manual rudder control while still
steer_state.locked_course = false;
@ -504,7 +504,7 @@ static bool auto_takeoff_check(void)
last_check_ms = now;
// Check for bad GPS
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) {
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
// no auto takeoff without GPS lock
return false;
}
@ -539,9 +539,9 @@ static bool auto_takeoff_check(void)
}
// Check ground speed and time delay
if (((g_gps->ground_speed_cm > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0)) &&
if (((gps.ground_speed() > g.takeoff_throttle_min_speed || g.takeoff_throttle_min_speed == 0.0)) &&
((now - last_tkoff_arm_time) >= min(uint16_t(g.takeoff_throttle_delay)*100,2500))) {
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed_cm*0.01f);
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), gps.ground_speed());
launchTimerStarted = false;
last_tkoff_arm_time = 0;
return true;
@ -565,9 +565,8 @@ no_launch:
static bool is_flying(void)
{
// If we don't have a GPS lock then don't use GPS for this test
bool gpsMovement = (g_gps == NULL ||
g_gps->status() < GPS::GPS_OK_FIX_2D ||
g_gps->ground_speed_cm >= 500);
bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D ||
gps.ground_speed() >= 5);
bool airspeedMovement = !airspeed.use() || airspeed.get_airspeed() >= 5;
@ -623,9 +622,8 @@ static bool suppress_throttle(void)
return false;
}
if (g_gps != NULL &&
g_gps->status() >= GPS::GPS_OK_FIX_2D &&
g_gps->ground_speed_cm >= 500) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
gps.ground_speed() >= 5) {
// if we have an airspeed sensor, then check it too, and
// require 5m/s. This prevents throttle up due to spiky GPS
// groundspeed with bad GPS reception

100
ArduPlane/GCS_Mavlink.pde
View File

@ -146,7 +146,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (airspeed.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) {
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
if (geofence_present()) {
@ -218,7 +218,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (g_gps != NULL && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ins.healthy()) {
@ -255,27 +255,28 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
static void NOINLINE send_location(mavlink_channel_t chan)
{
uint32_t fix_time;
uint32_t fix_time_ms;
// if we have a GPS fix, take the time as the last fix time. That
// allows us to correctly calculate velocities and extrapolate
// positions.
// If we don't have a GPS fix then we are dead reckoning, and will
// use the current boot time as the 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_ms = gps.last_fix_time_ms();
} else {
fix_time = millis();
fix_time_ms = hal.scheduler->millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
chan,
fix_time,
fix_time_ms,
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
g_gps->altitude_cm * 10, // millimeters above sea level
gps.location().alt * 10UL, // millimeters above sea level
relative_altitude() * 1.0e3, // millimeters above ground
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);
}
@ -295,39 +296,45 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
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);
#if GPS2_ENABLE
if (g_gps2 != NULL && g_gps2->status() != GPS::NO_GPS) {
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,
g_gps2->last_fix_time*(uint64_t)1000,
g_gps2->status(),
g_gps2->latitude, // in 1E7 degrees
g_gps2->longitude, // in 1E7 degrees
g_gps2->altitude_cm * 10, // in mm
g_gps2->hdop,
gps.last_fix_time_ms(1)*(uint64_t)1000,
gps.status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
gps.get_hdop(1),
65535,
g_gps2->ground_speed_cm, // cm/s
g_gps2->ground_course_cd, // 1/100 degrees,
g_gps2->num_sats,
gps.ground_speed(1)*100, // cm/s
gps.ground_course_cd(1), // 1/100 degrees,
gps.num_sats(1),
0,
0);
}
@ -339,7 +346,7 @@ static void NOINLINE send_system_time(mavlink_channel_t chan)
{
mavlink_msg_system_time_send(
chan,
g_gps->time_epoch_usec(),
gps.time_epoch_usec(),
hal.scheduler->millis());
}
@ -455,7 +462,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
mavlink_msg_vfr_hud_send(
chan,
aspeed,
(float)g_gps->ground_speed_cm * 0.01f,
gps.ground_speed(),
(ahrs.yaw_sensor / 100) % 360,
throttle,
current_loc.alt / 100.0,
@ -1580,16 +1587,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
last_hil_state = packet;
float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360_cd(ToDeg(atan2f(packet.vy, packet.vx)) * 100);
if (g_gps != NULL) {
// set gps hil sensor
g_gps->setHIL(GPS::FIX_3D,
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
gps.setHIL(AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
vel*1.0e-2, cog*1.0e-2, 0, 10);
}
loc, vel, 10);
// rad/sec
Vector3f gyros;

10
ArduPlane/Log.pde
View File

@ -253,8 +253,8 @@ static void Log_Write_Camera()
#if CAMERA == ENABLED
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
gps_time : g_gps->time_week_ms,
gps_week : g_gps->time_week,
gps_time : gps.time_week_ms(),
gps_week : gps.time_week(),
latitude : current_loc.lat,
longitude : current_loc.lng,
altitude : current_loc.alt,
@ -352,7 +352,7 @@ static void Log_Write_Nav_Tuning()
altitude_error_cm : (int16_t)altitude_error_cm,
airspeed_cm : (int16_t)airspeed.get_airspeed_cm(),
altitude : barometer.get_altitude(),
groundspeed_cm : g_gps->ground_speed_cm
groundspeed_cm : (uint32_t)(gps.ground_speed()*100)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -395,7 +395,7 @@ static void Log_Write_Sonar()
distance : sonar.distance_cm(),
voltage : sonar.voltage(),
baro_alt : barometer.get_altitude(),
groundspeed : (0.01f * g_gps->ground_speed_cm),
groundspeed : gps.ground_speed(),
throttle : (uint8_t)(100 * channel_throttle->norm_output())
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -493,7 +493,7 @@ static void Log_Write_Compass()
static void Log_Write_GPS(void)
{
DataFlash.Log_Write_GPS(g_gps, current_loc.alt - ahrs.get_home().alt);
DataFlash.Log_Write_GPS(gps, current_loc.alt - ahrs.get_home().alt);
}
static void Log_Write_IMU()

1
ArduPlane/Parameters.h
View File

@ -104,6 +104,7 @@ public:
k_param_rssi_range,
k_param_flapin_channel,
k_param_flaperon_output,
k_param_gps,
// 100: Arming parameters
k_param_arming = 100,

5
ArduPlane/Parameters.pde
View File

@ -805,6 +805,11 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
GOBJECT(barometer, "GND_", AP_Baro),
// GPS driver
// @Group: GPS_
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
GOBJECT(gps, "GPS_", AP_GPS),
#if CAMERA == ENABLED
// @Group: CAM_
// @Path: ../libraries/AP_Camera/AP_Camera.cpp

14
ArduPlane/commands.pde
View File

@ -97,17 +97,7 @@ static void init_home()
{
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
// block until we get a good fix
// -----------------------------
while (!g_gps->new_data || !g_gps->fix) {
g_gps->update();
#if HIL_MODE != HIL_MODE_DISABLED
// update hil gps so we have new_data
gcs_update();
#endif
}
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
ahrs.set_home(gps.location());
home_is_set = true;
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
@ -132,6 +122,6 @@ static void init_home()
*/
static void update_home()
{
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
ahrs.set_home(gps.location());
barometer.update_calibration();
}

8
ArduPlane/commands_logic.pde
View File

@ -359,7 +359,7 @@ static bool verify_land()
// Set land_complete if we are within 2 seconds distance or within
// 3 meters altitude of the landing point
if ((wp_distance <= (g.land_flare_sec*g_gps->ground_speed_cm*0.01f))
if ((wp_distance <= (g.land_flare_sec * gps.ground_speed()))
|| (adjusted_altitude_cm() <= next_WP_loc.alt + g.land_flare_alt*100)) {
land_complete = true;
@ -378,7 +378,7 @@ static bool verify_land()
gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), steer_state.hold_course_cd);
}
if (g_gps->ground_speed_cm*0.01f < 3.0) {
if (gps.ground_speed() < 3) {
// reload any airspeed or groundspeed parameters that may have
// been set for landing. We don't do this till ground
// speed drops below 3.0 m/s as otherwise we will change
@ -574,10 +574,10 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd)
static void do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) {
if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
init_home();
} else {
ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, cmd.content.location.alt*100.0f);
ahrs.set_home(cmd.content.location);
home_is_set = true;
}
}

20
ArduPlane/config.h
View File

@ -118,12 +118,6 @@
#endif
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#define GPS2_ENABLE 1
#else
#define GPS2_ENABLE 0
#endif
#ifndef CONFIG_BARO
# error "CONFIG_BARO not set"
#endif
@ -140,26 +134,12 @@
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef GPS_PROTOCOL
#define GPS_PROTOCOL GPS_PROTOCOL_HIL
#undef CONFIG_BARO
#define CONFIG_BARO AP_BARO_HIL
#undef CONFIG_INS_TYPE
#define CONFIG_INS_TYPE CONFIG_INS_HIL
#undef CONFIG_COMPASS
#define CONFIG_COMPASS AP_COMPASS_HIL
#undef GPS2_ENABLE
#define GPS2_ENABLE 0
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
#ifndef MAV_SYSTEM_ID

11
ArduPlane/defines.h
View File

@ -47,17 +47,6 @@ enum gcs_failsafe {
#define T6 1000000
#define T7 10000000
// GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0
#define GPS_PROTOCOL_SIRF 1
#define GPS_PROTOCOL_UBLOX 2
#define GPS_PROTOCOL_IMU 3
#define GPS_PROTOCOL_MTK 4
#define GPS_PROTOCOL_HIL 5
#define GPS_PROTOCOL_MTK19 6
#define GPS_PROTOCOL_AUTO 7
// HIL enumerations. Note that HIL_MODE_ATTITUDE and HIL_MODE_SENSORS
// are now the same thing, and are sensors based. The old define is
// kept to allow old APM_Config.h headers to keep working

8
ArduPlane/navigation.pde
View File

@ -118,7 +118,7 @@ static void calc_gndspeed_undershoot()
{
// Use the component of ground speed in the forward direction
// This prevents flyaway if wind takes plane backwards
if (g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
Vector2f gndVel = ahrs.groundspeed_vector();
const Matrix3f &rotMat = ahrs.get_dcm_matrix();
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
@ -169,8 +169,8 @@ static void update_cruise()
if (!cruise_state.locked_heading &&
channel_roll->control_in == 0 &&
channel_rudder->control_in == 0 &&
g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D &&
g_gps->ground_speed_cm >= 300 &&
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
gps.ground_speed() >= 3 &&
cruise_state.lock_timer_ms == 0) {
// user wants to lock the heading - start the timer
cruise_state.lock_timer_ms = hal.scheduler->millis();
@ -181,7 +181,7 @@ static void update_cruise()
// from user
cruise_state.locked_heading = true;
cruise_state.lock_timer_ms = 0;
cruise_state.locked_heading_cd = g_gps->ground_course_cd;
cruise_state.locked_heading_cd = gps.ground_course_cd();
prev_WP_loc = current_loc;
}
if (cruise_state.locked_heading) {

29
ArduPlane/system.pde
View File

@ -77,17 +77,6 @@ static void init_ardupilot()
//
hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
// GPS serial port.
//
// standard gps running
hal.uartB->begin(38400, 256, 16);
#if GPS2_ENABLE
if (hal.uartE != NULL) {
hal.uartE->begin(38400, 256, 16);
}
#endif
cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n"),
hal.util->available_memory());
@ -178,18 +167,8 @@ static void init_ardupilot()
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// Do GPS init
g_gps = &g_gps_driver;
// GPS Initialization
g_gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_4G, &DataFlash);
#if GPS2_ENABLE
if (hal.uartE != NULL) {
g_gps2 = &g_gps2_driver;
g_gps2->init(hal.uartE, GPS::GPS_ENGINE_AIRBORNE_4G, &DataFlash);
g_gps2->set_secondary();
}
#endif
gps.init(&DataFlash);
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
@ -290,12 +269,6 @@ static void startup_ground(void)
hal.uartD->set_blocking_writes(false);
}
#if 0
// leave GPS blocking until we have support for correct handling
// of GPS config in uBlox when non-blocking
hal.uartB->set_blocking_writes(false);
#endif
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}

39
ArduPlane/test.pde
View File

@ -20,7 +20,6 @@ static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -52,7 +51,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
{"adc", test_adc},
#endif
{"gps", test_gps},
{"rawgps", test_rawgps},
{"ins", test_ins},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
@ -406,18 +404,21 @@ test_gps(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
uint32_t last_message_time_ms = 0;
while(1) {
delay(100);
g_gps->update();
gps.update();
if (g_gps->new_data) {
if (gps.last_message_time_ms() != last_message_time_ms) {
last_message_time_ms = gps.last_message_time_ms();
const Location &loc = gps.location();
cliSerial->printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
(long)g_gps->latitude,
(long)g_gps->longitude,
(long)g_gps->altitude_cm/100,
(int)g_gps->num_sats);
}else{
(long)loc.lat,
(long)loc.lng,
(long)loc.alt/100,
(int)gps.num_sats());
} else {
cliSerial->printf_P(PSTR("."));
}
if(cliSerial->available() > 0) {
@ -609,26 +610,6 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1) {
// Blink Yellow LED if we are sending data to GPS
if (hal.uartC->available()) {
hal.uartB->write(hal.uartC->read());
}
// Blink Red LED if we are receiving data from GPS
if (hal.uartB->available()) {
hal.uartC->write(hal.uartB->read());
}
if(cliSerial->available() > 0) {
return (0);
}
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED