AP_GPS: added velocity_down() call

on GPSes with raw velocity support this will be used for DCM
correction
This commit is contained in:
Andrew Tridgell 2012-10-31 13:56:04 +11:00
parent 27607eacbd
commit e74e730992
2 changed files with 11 additions and 0 deletions

View File

@ -59,6 +59,7 @@ GPS::update(void)
// the GPS is able to give us velocity numbers directly // the GPS is able to give us velocity numbers directly
_velocity_north = _vel_north * 0.01; _velocity_north = _vel_north * 0.01;
_velocity_east = _vel_east * 0.01; _velocity_east = _vel_east * 0.01;
_velocity_down = _vel_down * 0.01;
} else { } else {
float gps_heading = ToRad(ground_course * 0.01); float gps_heading = ToRad(ground_course * 0.01);
float gps_speed = ground_speed * 0.01; float gps_speed = ground_speed * 0.01;
@ -69,6 +70,9 @@ GPS::update(void)
_velocity_north = gps_speed * cos_heading; _velocity_north = gps_speed * cos_heading;
_velocity_east = gps_speed * sin_heading; _velocity_east = gps_speed * sin_heading;
// no good way to get descent rate
_velocity_down = 0;
} }
} }
} }

View File

@ -128,6 +128,9 @@ public:
float velocity_east(void) { float velocity_east(void) {
return _status == GPS_OK ? _velocity_east : 0; return _status == GPS_OK ? _velocity_east : 0;
} }
float velocity_down(void) {
return _status == GPS_OK ? _velocity_down : 0;
}
// last ground speed in m/s. This can be used when we have no GPS // last ground speed in m/s. This can be used when we have no GPS
// lock to return the last ground speed we had with lock // lock to return the last ground speed we had with lock
@ -141,6 +144,9 @@ public:
// the time we got our last fix in system milliseconds // the time we got our last fix in system milliseconds
uint32_t last_fix_time; uint32_t last_fix_time;
// return true if the GPS supports raw velocity values
protected: protected:
Stream *_port; ///< port the GPS is attached to Stream *_port; ///< port the GPS is attached to
@ -222,6 +228,7 @@ private:
// components of the velocity, in m/s // components of the velocity, in m/s
float _velocity_north; float _velocity_north;
float _velocity_east; float _velocity_east;
float _velocity_down;
}; };
inline int32_t inline int32_t