38 #ifndef ROSFLIGHT_SENSORS_CALBRATE_MAG_H 39 #define ROSFLIGHT_SENSORS_CALBRATE_MAG_H 44 #include <rosflight_msgs/ParamSet.h> 46 #include <sensor_msgs/MagneticField.h> 49 #include <eigen3/Eigen/Eigen> 52 #include <boost/bind.hpp> 80 bool mag_callback(
const sensor_msgs::MagneticField::ConstPtr &mag);
91 const double a11()
const {
return A_(0, 0); }
92 const double a12()
const {
return A_(0, 1); }
93 const double a13()
const {
return A_(0, 2); }
94 const double a21()
const {
return A_(1, 0); }
95 const double a22()
const {
return A_(1, 1); }
96 const double a23()
const {
return A_(1, 2); }
97 const double a31()
const {
return A_(2, 0); }
98 const double a32()
const {
return A_(2, 1); }
99 const double a33()
const {
return A_(2, 2); }
100 const double bx()
const {
return b_(0, 0); }
101 const double by()
const {
return b_(1, 0); }
102 const double bz()
const {
return b_(2, 0); }
105 bool set_param(std::string name,
double value);
134 Eigen::Vector3d
intersect(Eigen::Vector3d r_m, Eigen::Vector3d r_e, Eigen::MatrixXd Q, Eigen::MatrixXd ub,
double k);
139 void eigSort(Eigen::MatrixXd &w, Eigen::MatrixXd &v);
153 void magCal(Eigen::MatrixXd u, Eigen::MatrixXd &A, Eigen::MatrixXd &bb);
158 #endif // MAVROSFLIGHT_SENSORS_MAG_H
Eigen::Vector3d intersect(Eigen::Vector3d r_m, Eigen::Vector3d r_e, Eigen::MatrixXd Q, Eigen::MatrixXd ub, double k)
message_filters::Subscriber< sensor_msgs::MagneticField > mag_subscriber_
CalibrateMag sensor class.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
double calibration_time_
seconds to record data for temperature compensation
bool calibrating_
whether a temperature calibration is in progress
void set_reference_magnetic_field_strength(double reference_magnetic_field)
ros::NodeHandle nh_private_
bool first_time_
waiting for first measurement for calibration
int ransac_iters_
number of ransac iterations to fit ellipsoid to mag measurements
ros::ServiceServer mag_cal_srv_
Eigen::MatrixXd ellipsoidRANSAC(EigenSTL::vector_Vector3d meas, int iters, double inlier_thresh)
bool set_param(std::string name, double value)
ros::ServiceClient param_set_client_
Eigen::Vector3d measurement_prev_
const double a11() const
The const stuff is to make it read-only.
double inlier_thresh_
threshold to consider measurement an inlier in ellipsoidRANSAC
double start_time_
timestamp of first calibration measurement
void magCal(Eigen::MatrixXd u, Eigen::MatrixXd &A, Eigen::MatrixXd &bb)
void do_mag_calibration()
Eigen::MatrixXd ellipsoidLS(EigenSTL::vector_Vector3d meas)
bool is_calibrating()
Check if a calibration is in progress.
void start_mag_calibration()
Begin the magnetometer calibration routine.
bool mag_callback(const sensor_msgs::MagneticField::ConstPtr &mag)
set_refence_magnetic_field_strength
EigenSTL::vector_Vector3d measurements_
int measurement_throttle_
double reference_field_strength_
the strength of earth's magnetic field at your location
void eigSort(Eigen::MatrixXd &w, Eigen::MatrixXd &v)