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

AP_Compass: allow setting of exact timestamp in HIL compass

Browse Source
This commit is contained in:
Andrew Tridgell 2016-05-04 17:11:02 +10:00
parent 4318fd0ab8
commit 88a1ebaf0e
5 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass.cpp
View File

@ -761,11 +761,11 @@ void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
// Update raw magnetometer values from HIL mag vector
//
void Compass::setHIL(uint8_t instance, const Vector3f &mag)
void Compass::setHIL(uint8_t instance, const Vector3f &mag, uint32_t update_usec)
{
_hil.field[instance] = mag;
_hil.healthy[instance] = true;
_state[instance].last_update_usec = AP_HAL::micros();
_state[instance].last_update_usec = update_usec;
}
const Vector3f& Compass::getHIL(uint8_t instance) const

2
libraries/AP_Compass/AP_Compass.h
View File

@ -273,7 +273,7 @@ public:
// HIL methods
void setHIL(uint8_t instance, float roll, float pitch, float yaw);
void setHIL(uint8_t instance, const Vector3f &mag);
void setHIL(uint8_t instance, const Vector3f &mag, uint32_t last_update_usec);
const Vector3f& getHIL(uint8_t instance) const;
void _setup_earth_field();

6
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -84,6 +84,12 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins
state.last_update_usec = AP_HAL::micros();
}
void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
state.last_update_usec = last_update;
}
/*
register a new backend with frontend, returning instance which
should be used in publish_field()

1
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -61,6 +61,7 @@ protected:
void publish_raw_field(const Vector3f &mag, uint32_t time_us, uint8_t instance);
void correct_field(Vector3f &mag, uint8_t i);
void publish_filtered_field(const Vector3f &mag, uint8_t instance);
void set_last_update_usec(uint32_t last_update, uint8_t instance);
// register a new compass instance with the frontend
uint8_t register_compass(void) const;

2
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -66,7 +66,9 @@ void AP_Compass_HIL::read()
rotate_field(field, compass_instance);
publish_raw_field(field, AP_HAL::micros(), compass_instance);
correct_field(field, compass_instance);
uint32_t saved_last_update = _compass.last_update_usec(compass_instance);
publish_filtered_field(field, compass_instance);
set_last_update_usec(saved_last_update, compass_instance);
}
}
}