35 #include "std_msgs/String.h"
43 #include <boost/format.hpp>
45 #include <imu_network/sensors_network.h>
46 #include <imu_network/filtered_imu_network.h>
47 #include <imu_network/filtered_imu.h>
49 #define MAX_MEAN_IT 25 //number of iterations to measure the mean values from gyroscopes
51 #define GYRO_CONVERT M_PI/180*0.069565 // conversion of gyro's values from ADC to rad/sec
92 imu_network::filtered_imu filtered_imu_data;
95 ax = msg->sensors_val[i].S_Ax;
96 ay = msg->sensors_val[i].S_Ay;
97 az = msg->sensors_val[i].S_Az;
99 accel_ang_x.push_back(atan(ay/sqrt((ax*ax)+(az*az))));
100 accel_ang_y.push_back(atan(ax/sqrt((ay*ay)+(az*az))));
114 angle[0].push_back(0);
115 angle[1].push_back(0);
116 angle[2].push_back(0);
163 sensor_var[i][0]=msg->sensors_val[i].S_Ax;
164 sensor_var[i][1]=msg->sensors_val[i].S_Ay;
165 sensor_var[i][2]=msg->sensors_val[i].S_Az;
166 sensor_var[i][3]=msg->sensors_val[i].S_Mx;
167 sensor_var[i][4]=msg->sensors_val[i].S_My;
168 sensor_var[i][5]=msg->sensors_val[i].S_Mz;
169 sensor_var[i][6]=msg->sensors_val[i].S_Gx;
170 sensor_var[i][7]=msg->sensors_val[i].S_Gy;
171 sensor_var[i][8]=msg->sensors_val[i].S_Gz;
174 ax = sensor_var[i][0];
175 ay = sensor_var[i][1];
176 az = sensor_var[i][2];
189 float accel_magnitude = sqrt(ax*ax + ay*ay + az*az);
228 int main(
int argc,
char **argv)
230 ros::init(argc, argv,
"complementary_imu");
231 n =
new(ros::NodeHandle);
232 ros::Subscriber sub =
n->subscribe(
"topic_raw_data", 1000,
chatterCallback);
233 chatter_pub =
n->advertise<imu_network::filtered_imu_network>(
"topic_filtered_imu", 1000);
vector< float > gyro_angle[3]
vector< float > accel_ang_y
vector< float > angle_last_it[3]
ros::Publisher chatter_pub
void chatterCallback(const imu_network::sensors_network::ConstPtr &msg)
vector< float > gyro_mean_vect[MAX_MEAN_IT][3]
imu_network::filtered_imu_network complementary_filter_network
vector< float > gyro_last_it[3]
vector< float > accel_ang_x
vector< float > gyro_real_mean[3]
int main(int argc, char **argv)