void cb_imu(){
msg_timestamp = nh.now();
orientation = bno.getQuat();
orientation_msg.header.seq = msg_seq;
orientation_msg.header.stamp = msg_timestamp;
orientation_msg.quaternion.x = orientation.x();
orientation_msg.quaternion.y = orientation.y();
orientation_msg.quaternion.z = orientation.z();
orientation_msg.quaternion.w = orientation.w();
pub_orientation.publish(&orientation_msg);
angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
angular_vel_msg.header.seq = msg_seq;
angular_vel_msg.header.stamp = msg_timestamp;
angular_vel_msg.vector.x = angular_vel.x();
angular_vel_msg.vector.y = angular_vel.y();
angular_vel_msg.vector.z = angular_vel.z();
pub_angular_vel.publish(&angular_vel_msg);
linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
linear_accel_msg.header.seq = msg_seq;
linear_accel_msg.header.stamp = msg_timestamp;
linear_accel_msg.vector.x = linear_accel.x();
linear_accel_msg.vector.y = linear_accel.y();
linear_accel_msg.vector.z = linear_accel.z();
pub_linear_accel.publish(&linear_accel_msg);
msg_seq++;
}
void loop() {
if (s_report_button1_state)
{
bool button1_state = digitalRead(BUTTON1_PIN);
Particle.publish("button1", String(button1_state));
s_report_button1_state = false;
}
if (s_report_button2_state)
{
bool button2_state = digitalRead(BUTTON2_PIN);
Particle.publish("button2", String(button2_state));
s_report_button2_state = false;
}
if(s_start_battery)
{
setup_battery();
s_start_battery = false;
}
if (s_report_battery_state)
{
double voltage = lipo.getVoltage();
double soc = lipo.getSOC();
Particle.publish("battery", "volt: " + String(voltage) + " pct: " + String(soc));
s_report_battery_state = false;
}
// bno1
// =====
if(s_setup_bno055_1)
{
setup_bno055_1();
}
if(s_report_imu_state_1)
{
uint8_t system, gyro, accel, mag = 0;
bno1.getCalibration(&system, &gyro, &accel, &mag);
imu::Vector<3> euler = bno1.getVector(Adafruit_BNO055::VECTOR_EULER);
String status = String::format("Orient: x=%.0f y=%.0f z=%.0f Cal s:%d g:%d a:%d m:%d",
euler.x(),
euler.y(),
euler.z(),
system,
gyro,
accel,
mag
);
Particle.publish("imu1", status);
s_report_imu_state_1 = false;
}
// bno2
// =====
if(s_setup_bno055_2)
{
setup_bno055_2();
}
if(s_report_imu_state_2)
{
uint8_t system, gyro, accel, mag = 0;
bno2.getCalibration(&system, &gyro, &accel, &mag);
imu::Vector<3> euler = bno2.getVector(Adafruit_BNO055::VECTOR_EULER);
String status = String::format("Orient: x=%.0f y=%.0f z=%.0f Cal s:%d g:%d a:%d m:%d",
euler.x(),
euler.y(),
euler.z(),
system,
gyro,
accel,
mag
);
Particle.publish("imu2", status);
s_report_imu_state_2 = false;
}
delay(250);
}