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

Copter: convert to new GPS API

This commit is contained in:
Andrew Tridgell 2014-03-31 18:07:46 +11:00
parent f4079f57b2
commit 640b64f5e4
12 changed files with 103 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -23,8 +23,6 @@
# define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
#endif
//#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space
//#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX // hard code GPS to Ublox to save 8k of flash
//#define GPS_PROTOCOL GPS_PROTOCOL_MTK19 // hard cdoe GPS to Mediatek to save 10k of flash
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space

5
ArduCopter/APM_Config_mavlink_hil.h
View File

@ -28,11 +28,6 @@
#define HIL_PORT 3
// You can set your gps protocol here for your actual
// hardware and leave it without affecting the hardware
// in the loop simulation
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
// Sensors
// All sensors are supported in all modes.
#define MAGNETOMETER ENABLED

130
ArduCopter/ArduCopter.pde
View File

@ -237,12 +237,9 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
// 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
static GPS_Glitch gps_glitch(g_gps);
static AP_GPS gps;
static GPS_Glitch gps_glitch(gps);
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
@ -295,41 +292,11 @@ static AP_Compass_HMC5843 compass;
#endif
#endif
// real 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;
#else
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#elif HIL_MODE != HIL_MODE_DISABLED
// sensor emulators
static AP_ADC_HIL adc;
static AP_Baro_HIL barometer;
static AP_Compass_HIL compass;
static AP_GPS_HIL g_gps_driver;
static AP_InertialSensor_HIL ins;
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
@ -343,9 +310,9 @@ static SITL sitl;
// 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
// Mission library
@ -655,9 +622,9 @@ static float G_Dt = 0.02;
// Inertial Navigation
////////////////////////////////////////////////////////////////////////////////
#if AP_AHRS_NAVEKF_AVAILABLE
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, g_gps, gps_glitch);
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
#else
static AP_InertialNav inertial_nav(ahrs, barometer, g_gps, gps_glitch);
static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -716,14 +683,12 @@ static AP_HAL::AnalogSource* rssi_analog_source;
// --------------------------------------
#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
////////////////////////////////////////////////////////////////////////////////
@ -1203,15 +1168,15 @@ static void update_GPS(void)
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location
bool report_gps_glitch;
g_gps->update();
gps.update();
// logging and glitch protection run after every gps message
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();
// log GPS message
if (g.log_bitmask & MASK_LOG_GPS) {
DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
DataFlash.Log_Write_GPS(gps, current_loc.alt);
}
// run glitch protection and update AP_Notify if home has been initialised
@ -1227,61 +1192,46 @@ static void update_GPS(void)
AP_Notify::flags.gps_glitching = report_gps_glitch;
}
}
}
// checks to initialise home and take location based pictures
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
// clear new data flag
g_gps->new_data = false;
// checks to initialise home and take location based pictures
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// check if we can initialise home yet
if (!ap.home_is_set) {
// if we have a 3d lock and valid location
if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) {
if( ground_start_count > 0 ) {
ground_start_count--;
}else{
// after 10 successful reads store home location
// ap.home_is_set will be true so this will only happen once
ground_start_count = 0;
init_home();
// check if we can initialise home yet
if (!ap.home_is_set) {
// if we have a 3d lock and valid location
if(gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.location().lat != 0) {
if (ground_start_count > 0 ) {
ground_start_count--;
} else {
// after 10 successful reads store home location
// ap.home_is_set will be true so this will only happen once
ground_start_count = 0;
init_home();
// set system clock for log timestamps
hal.util->set_system_clock(g_gps->time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
// set system clock for log timestamps
hal.util->set_system_clock(gps.time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
}
} else {
// start again if we lose 3d lock
ground_start_count = 10;
}
}else{
// start again if we lose 3d lock
ground_start_count = 10;
}
}
#if CAMERA == ENABLED
if (camera.update_location(current_loc) == true) {
do_take_picture();
}
if (camera.update_location(current_loc) == true) {
do_take_picture();
}
#endif
}
}
// check for loss of gps
failsafe_gps_check();
#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 (g.log_bitmask & MASK_LOG_GPS) {
DataFlash.Log_Write_GPS2(g_gps2);
}
}
}
#endif
}
static void

4
ArduCopter/Attitude.pde
View File

@ -67,8 +67,8 @@ static float get_roi_yaw()
static float get_look_ahead_yaw()
{
// Commanded Yaw to automatically look ahead.
if (g_gps->fix && g_gps->ground_speed_cm > YAW_LOOK_AHEAD_MIN_SPEED) {
yaw_look_ahead_bearing = g_gps->ground_course_cd;
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed_cm() > YAW_LOOK_AHEAD_MIN_SPEED) {
yaw_look_ahead_bearing = gps.ground_course_cd();
}
return yaw_look_ahead_bearing;
}

89
ArduCopter/GCS_Mavlink.pde
View File

@ -151,7 +151,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
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 OPTFLOW == ENABLED
@ -188,7 +188,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::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
if (gps.status() > AP_GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
if (ap.rc_receiver_present && !failsafe.radio) {
@ -229,21 +229,22 @@ static void NOINLINE send_location(mavlink_channel_t chan)
// 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 = gps.last_fix_time_ms();
} else {
fix_time = millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
chan,
fix_time,
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
(current_loc.alt - home.alt) * 10, // 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.x * -100, // Z speed cm/s (+ve up)
ahrs.yaw_sensor); // compass heading in 1/100 degree
}
@ -295,36 +296,39 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
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 GPS2_ENABLE
if (g_gps2 != NULL && g_gps2->status() != GPS::NO_GPS) {
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
if (gps.num_sensors() > 1 &&
gps.status(1) > AP_GPS::NO_GPS) {
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
const Location &loc2 = gps.location(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),
loc2.lat,
loc2.lng,
loc2.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,
0,
gps.ground_speed(1)*100, // cm/s
gps.ground_course_cd(1), // 1/100 degrees,
gps.num_sats(1),
0,
0);
}
}
@ -335,7 +339,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());
}
@ -484,8 +488,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)g_gps->ground_speed_cm / 100.0f,
(float)g_gps->ground_speed_cm / 100.0f,
gps.ground_speed(),
gps.ground_speed(),
(ahrs.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
current_loc.alt / 100.0f,
@ -1339,18 +1343,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360_cd(ToDeg(atan2f(packet.vx, packet.vy)) * 100);
// if we are erasing the dataflash this object doesnt exist yet. as its called from delay_cb
if (g_gps == NULL)
break;
// set gps hil sensor
g_gps->setHIL(GPS::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);
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,
loc, vel, 10);
if (!ap.home_is_set) {
init_home();

4
ArduCopter/Log.pde
View File

@ -627,8 +627,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,

7
ArduCopter/commands.pde
View File

@ -5,7 +5,12 @@
static void init_home()
{
set_home_is_set(true);
ahrs.set_home(g_gps->latitude, g_gps->longitude, 0);
// copter uses 0 home altitude
Location loc = gps.location();
loc.alt = 0;
ahrs.set_home(loc);
inertial_nav.setup_home_position();

4
ArduCopter/commands_logic.pde
View File

@ -708,7 +708,9 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
if(cmd.p1 == 1) {
init_home();
} else {
ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, 0);
Location loc = cmd.content.location;
loc.alt = 0;
ahrs.set_home(loc);
set_home_is_set(true);
}
}

21
ArduCopter/config.h
View File

@ -107,14 +107,6 @@
# define MAIN_LOOP_MICROS 10000
#endif
// 2nd GPS support
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#define GPS2_ENABLE 1
#else
#define GPS2_ENABLE 0
#endif
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
//
@ -269,22 +261,9 @@
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#undef CONFIG_SONAR
#define CONFIG_SONAR DISABLED
#undef GPS2_ENABLE
#define GPS2_ENABLE 0
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif

11
ArduCopter/defines.h
View File

@ -77,17 +77,6 @@
#define ToRad(x) radians(x) // *pi/180
#define ToDeg(x) degrees(x) // *180/pi
// 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
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1

2
ArduCopter/motors.pde
View File

@ -436,7 +436,7 @@ static bool pre_arm_gps_checks(bool display_failure)
}
// warn about hdop separately - to prevent user confusion with no gps lock
if (g_gps->hdop > g.gps_hdop_good) {
if (gps.get_hdop() > g.gps_hdop_good) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
}

15
ArduCopter/system.pde
View File

@ -227,17 +227,7 @@ static void init_ardupilot()
#endif // CONFIG_ADC
// 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);
if(g.compass_enabled)
init_compass();
@ -338,7 +328,8 @@ static void startup_ground(bool force_gyro_cal)
// returns true if the GPS is ok and home position is set
static bool GPS_ok()
{
if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D && !gps_glitch.glitching() && !failsafe.gps) {
if (ap.home_is_set && gps.status() == AP_GPS::GPS_OK_FIX_3D &&
!gps_glitch.glitching() && !failsafe.gps) {
return true;
}else{
return false;