Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
A
Ardupilot
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
OpenSource
Ardupilot
Commits
cf69da59
Commit
cf69da59
authored
12 years ago
by
uncrustify
Committed by
Pat Hickey
12 years ago
Browse files
Options
Downloads
Patches
Plain Diff
uncrustify libraries/AP_Compass/Compass.h
parent
8f9bb7f9
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
libraries/AP_Compass/Compass.h
+99
-95
99 additions, 95 deletions
libraries/AP_Compass/Compass.h
with
99 additions
and
95 deletions
libraries/AP_Compass/Compass.h
+
99
−
95
View file @
cf69da59
...
...
@@ -16,116 +16,120 @@
class
Compass
{
public:
int16_t
product_id
;
/// product id
int16_t
mag_x
;
///< magnetic field strength along the X axis
int16_t
mag_y
;
///< magnetic field strength along the Y axis
int16_t
mag_z
;
///< magnetic field strength along the Z axis
uint32_t
last_update
;
///< micros() time of last update
bool
healthy
;
///< true if last read OK
/// Constructor
///
Compass
();
/// Initialize the compass device.
///
/// @returns True if the compass was initialized OK, false if it was not
/// found or is not functioning.
///
virtual
bool
init
();
/// Read the compass and update the mag_ variables.
///
virtual
bool
read
(
void
)
=
0
;
/// Calculate the tilt-compensated heading_ variables.
///
/// @param roll The current airframe roll angle.
/// @param pitch The current airframe pitch angle.
///
int16_t
product_id
;
/// product id
int16_t
mag_x
;
///< magnetic field strength along the X axis
int16_t
mag_y
;
///< magnetic field strength along the Y axis
int16_t
mag_z
;
///< magnetic field strength along the Z axis
uint32_t
last_update
;
///< micros() time of last update
bool
healthy
;
///< true if last read OK
/// Constructor
///
Compass
();
/// Initialize the compass device.
///
/// @returns True if the compass was initialized OK, false if it was not
/// found or is not functioning.
///
virtual
bool
init
();
/// Read the compass and update the mag_ variables.
///
virtual
bool
read
(
void
)
=
0
;
/// Calculate the tilt-compensated heading_ variables.
///
/// @param roll The current airframe roll angle.
/// @param pitch The current airframe pitch angle.
///
/// @returns heading in radians
///
virtual
float
calculate_heading
(
float
roll
,
float
pitch
);
virtual
float
calculate_heading
(
float
roll
,
float
pitch
);
/// Calculate the tilt-compensated heading_ variables.
///
/// @param dcm_matrix The current orientation rotation matrix
/// Calculate the tilt-compensated heading_ variables.
///
/// @param dcm_matrix The current orientation rotation matrix
///
/// @returns heading in radians
///
virtual
float
calculate_heading
(
const
Matrix3f
&
dcm_matrix
);
/// Set the compass orientation matrix, used to correct for
/// various compass mounting positions.
///
/// @param rotation_matrix Rotation matrix to transform magnetometer readings
/// to the body frame.
///
virtual
void
set_orientation
(
enum
Rotation
rotation
);
/// Sets the compass offset x/y/z values.
///
/// @param offsets Offsets to the raw mag_ values.
///
virtual
void
set_offsets
(
const
Vector3f
&
offsets
);
/// Saves the current compass offset x/y/z values.
///
/// This should be invoked periodically to save the offset values maintained by
/// ::null_offsets.
///
virtual
void
save_offsets
();
/// Returns the current offset values
///
/// @returns The current compass offsets.
///
virtual
Vector3f
&
get_offsets
();
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
/// @param longitude GPS Longitude.
///
void
set_initial_location
(
int32_t
latitude
,
int32_t
longitude
);
/// Program new offset values.
///
/// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value.
///
void
set_offsets
(
int
x
,
int
y
,
int
z
)
{
set_offsets
(
Vector3f
(
x
,
y
,
z
));
}
/// Perform automatic offset updates
///
void
null_offsets
(
void
);
///
virtual
float
calculate_heading
(
const
Matrix3f
&
dcm_matrix
);
/// Set the compass orientation matrix, used to correct for
/// various compass mounting positions.
///
/// @param rotation_matrix Rotation matrix to transform magnetometer readings
/// to the body frame.
///
virtual
void
set_orientation
(
enum
Rotation
rotation
);
/// Sets the compass offset x/y/z values.
///
/// @param offsets Offsets to the raw mag_ values.
///
virtual
void
set_offsets
(
const
Vector3f
&
offsets
);
/// Saves the current compass offset x/y/z values.
///
/// This should be invoked periodically to save the offset values maintained by
/// ::null_offsets.
///
virtual
void
save_offsets
();
/// Returns the current offset values
///
/// @returns The current compass offsets.
///
virtual
Vector3f
&
get_offsets
();
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
/// @param longitude GPS Longitude.
///
void
set_initial_location
(
int32_t
latitude
,
int32_t
longitude
);
/// Program new offset values.
///
/// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value.
///
void
set_offsets
(
int
x
,
int
y
,
int
z
)
{
set_offsets
(
Vector3f
(
x
,
y
,
z
));
}
/// Perform automatic offset updates
///
void
null_offsets
(
void
);
/// return true if the compass should be used for yaw calculations
bool
use_for_yaw
(
void
)
{
return
healthy
&&
_use_for_yaw
;
}
bool
use_for_yaw
(
void
)
{
return
healthy
&&
_use_for_yaw
;
}
/// Sets the local magnetic field declination.
///
/// @param radians Local field declination.
///
virtual
void
set_declination
(
float
radians
);
float
get_declination
();
/// Sets the local magnetic field declination.
///
/// @param radians Local field declination.
///
virtual
void
set_declination
(
float
radians
);
float
get_declination
();
static
const
struct
AP_Param
::
GroupInfo
var_info
[];
protected
:
enum
Rotation
_orientation
;
AP_Vector3f
_offset
;
AP_Float
_declination
;
AP_Int8
_learn
;
///<enable calibration learning
AP_Int8
_use_for_yaw
;
///<enable use for yaw calculation
AP_Int8
_auto_declination
;
///<enable automatic declination code
enum
Rotation
_orientation
;
AP_Vector3f
_offset
;
AP_Float
_declination
;
AP_Int8
_learn
;
///<enable calibration learning
AP_Int8
_use_for_yaw
;
///<enable use for yaw calculation
AP_Int8
_auto_declination
;
///<enable automatic declination code
bool
_null_init_done
;
///< first-time-around flag used by offset nulling
bool
_null_init_done
;
///< first-time-around flag used by offset nulling
///< used by offset correction
static
const
uint8_t
_mag_history_size
=
20
;
uint8_t
_mag_history_index
;
Vector3i
_mag_history
[
_mag_history_size
];
uint8_t
_mag_history_index
;
Vector3i
_mag_history
[
_mag_history_size
];
};
#endif
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment