Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: change ground course to be in degrees

Browse Source 
more accuracy for replay
This commit is contained in:
Andrew Tridgell 2016-05-05 11:28:35 +10:00
parent e8142b0b5b
commit b424c49bc7
14 changed files with 31 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_GPS/AP_GPS.cpp
View File

@ -467,7 +467,7 @@ AP_GPS::update(void)
void
AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
const Location &_location, const Vector3f &_velocity, uint8_t _num_sats,
uint16_t hdop, bool _have_vertical_velocity)
uint16_t hdop)
{
if (instance >= GPS_MAX_INSTANCES) {
return;
@ -479,10 +479,9 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
istate.location.options = 0;
istate.velocity = _velocity;
istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);
istate.ground_course_cd = wrap_360_cd(degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL);
istate.ground_course = wrap_360(degrees(atan2f(istate.velocity.y, istate.velocity.x)));
istate.hdop = hdop;
istate.num_sats = _num_sats;
istate.have_vertical_velocity |= _have_vertical_velocity;
istate.last_gps_time_ms = tnow;
uint64_t gps_time_ms = time_epoch_ms - (17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL);
istate.time_week = gps_time_ms / (86400*7*(uint64_t)1000);
@ -493,7 +492,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
}
// set accuracy for HIL
void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc)
void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity)
{
GPS_State &istate = state[instance];
istate.vdop = vdop;
@ -503,6 +502,7 @@ void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vac
istate.have_horizontal_accuracy = true;
istate.have_vertical_accuracy = true;
istate.have_speed_accuracy = true;
istate.have_vertical_velocity |= _have_vertical_velocity;
}
/**
@ -574,7 +574,7 @@ AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
get_hdop(0),
get_vdop(0),
ground_speed(0)*100, // cm/s
ground_course_cd(0), // 1/100 degrees,
ground_course(0)*100, // 1/100 degrees,
num_sats(0));
}
@ -602,7 +602,7 @@ AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
get_hdop(1),
get_vdop(1),
ground_speed(1)*100, // cm/s
ground_course_cd(1), // 1/100 degrees,
ground_course(1)*100, // 1/100 degrees,
num_sats(1),
0,
0);

14
libraries/AP_GPS/AP_GPS.h
View File

@ -112,7 +112,7 @@ public:
uint16_t time_week; ///< GPS week number
Location location; ///< last fix location
float ground_speed; ///< ground speed in m/sec
int32_t ground_course_cd; ///< ground course in 100ths of a degree
float ground_course; ///< ground course in degrees
uint16_t hdop; ///< horizontal dilution of precision in cm
uint16_t vdop; ///< vertical dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
@ -217,8 +217,14 @@ public:
}
// ground course in centidegrees
float ground_course(uint8_t instance) const {
return state[instance].ground_course;
}
float ground_course() const {
return ground_course(primary_instance);
}
int32_t ground_course_cd(uint8_t instance) const {
return state[instance].ground_course_cd;
return ground_course(instance) * 100;
}
int32_t ground_course_cd() const {
return ground_course_cd(primary_instance);
@ -305,10 +311,10 @@ public:
// set position for HIL
void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms,
const Location &location, const Vector3f &velocity, uint8_t num_sats,
uint16_t hdop, bool _have_vertical_velocity);
uint16_t hdop);
// set accuracy for HIL
void setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc);
void setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity);
static const struct AP_Param::GroupInfo var_info[];

2
libraries/AP_GPS/AP_GPS_ERB.cpp
View File

@ -201,7 +201,7 @@ AP_GPS_ERB::_parse_gps(void)
_last_vel_time = _buffer.vel.time;
state.ground_speed = _buffer.vel.speed_2d * 0.01f; // m/s
// Heading 2D deg * 100000 rescaled to deg * 100
state.ground_course_cd = wrap_360_cd(_buffer.vel.heading_2d / 1000);
state.ground_course = wrap_360(_buffer.vel.heading_2d * 1.0e-5f);
state.have_vertical_velocity = true;
state.velocity.x = _buffer.vel.vel_north * 0.01f;
state.velocity.y = _buffer.vel.vel_east * 0.01f;

2
libraries/AP_GPS/AP_GPS_GSOF.cpp
View File

@ -310,7 +310,7 @@ AP_GPS_GSOF::process_message(void)
if ((vflag & 1) == 1)
{
state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100);
state.ground_course = degrees(SwapFloat(gsof_msg.data, a + 5));
fill_3d_velocity();
state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
state.have_vertical_velocity = true;

2
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -147,7 +147,7 @@ restart:
state.location.lng = swap_int32(_buffer.msg.longitude) * 10;
state.location.alt = swap_int32(_buffer.msg.altitude);
state.ground_speed = swap_int32(_buffer.msg.ground_speed) * 0.01f;
state.ground_course_cd = wrap_360_cd(swap_int32(_buffer.msg.ground_course) / 10000);
state.ground_course = wrap_360(swap_int32(_buffer.msg.ground_course) * 1.0e-6f);
state.num_sats = _buffer.msg.satellites;
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {

2
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -145,7 +145,7 @@ restart:
}
state.location.alt = _buffer.msg.altitude;
state.ground_speed = _buffer.msg.ground_speed*0.01f;
state.ground_course_cd = wrap_360_cd(_buffer.msg.ground_course);
state.ground_course = wrap_360(_buffer.msg.ground_course*0.01f);
state.num_sats = _buffer.msg.satellites;
state.hdop = _buffer.msg.hdop;

6
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -291,7 +291,7 @@ bool AP_GPS_NMEA::_term_complete()
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = wrap_360_cd(_new_course);
state.ground_course = wrap_360(_new_course*0.01f);
make_gps_time(_new_date, _new_time * 10);
state.last_gps_time_ms = now;
// To-Do: add support for proper reporting of 2D and 3D fix
@ -310,8 +310,8 @@ bool AP_GPS_NMEA::_term_complete()
break;
case _GPS_SENTENCE_VTG:
_last_VTG_ms = now;
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = wrap_360_cd(_new_course);
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
fill_3d_velocity();
// VTG has no fix indicator, can't change fix status
break;

2
libraries/AP_GPS/AP_GPS_PX4.cpp
View File

@ -63,7 +63,7 @@ AP_GPS_PX4::read(void)
state.location.alt = _gps_pos.alt/10;
state.ground_speed = _gps_pos.vel_m_s;
state.ground_course_cd = wrap_360_cd(degrees(_gps_pos.cog_rad)*100);
state.ground_course = wrap_360(degrees(_gps_pos.cog_rad));
state.hdop = _gps_pos.eph*100;
// convert epoch timestamp back to gps epoch - evil hack until we get the genuine

4
libraries/AP_GPS/AP_GPS_QURT.cpp
View File

@ -88,7 +88,7 @@ AP_GPS_QURT::read(void)
state.location.alt = data.alt_from_MSL;
state.ground_speed = data.speed_over_ground;
state.ground_course_cd = data.course_over_ground;
state.ground_course = data.course_over_ground*0.01f;
// convert epoch timestamp back to gps epoch - evil hack until we get the genuine
// raw week information (or APM switches to Posix epoch ;-) )
@ -101,7 +101,7 @@ AP_GPS_QURT::read(void)
}
state.have_vertical_velocity = true;
float gps_heading = radians(state.ground_course_cd * 0.01f);
float gps_heading = radians(state.ground_course);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading);
state.velocity.z = -data.climb_rate;

3
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -226,8 +226,7 @@ AP_GPS_SBF::process_message(void)
float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
state.ground_speed = (float)safe_sqrt(ground_vector_sq);
state.ground_course_cd = (int32_t)(100 * ToDeg(atan2f(state.velocity[1], state.velocity[0])));
state.ground_course_cd = wrap_360_cd(state.ground_course_cd);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.horizontal_accuracy = (float)temp.HAccuracy * 0.01f;
state.vertical_accuracy = (float)temp.VAccuracy * 0.01f;

2
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -272,7 +272,7 @@ AP_GPS_SBP::_attempt_state_update()
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course_cd = wrap_360_cd((int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0])));
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
// Update position state

2
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -186,7 +186,7 @@ AP_GPS_SIRF::_parse_gps(void)
state.location.lng = swap_int32(_buffer.nav.longitude);
state.location.alt = swap_int32(_buffer.nav.altitude_msl);
state.ground_speed = swap_int32(_buffer.nav.ground_speed)*0.01f;
state.ground_course_cd = wrap_360_cd(swap_int16(_buffer.nav.ground_course));
state.ground_course = wrap_360(swap_int16(_buffer.nav.ground_course)*0.01f);
state.num_sats = _buffer.nav.satellites;
fill_3d_velocity();
return true;

2
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -923,7 +923,7 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_VELNED");
_last_vel_time = _buffer.velned.time;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course_cd = wrap_360_cd(_buffer.velned.heading_2d / 1000); // Heading 2D deg * 100000 rescaled to deg * 100
state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;

2
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -131,7 +131,7 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
*/
void AP_GPS_Backend::fill_3d_velocity(void)
{
float gps_heading = ToRad(state.ground_course_cd * 0.01f);
float gps_heading = radians(state.ground_course);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading);