uint8_t send_gyro_can(uint8_t type)
{
struct mcp2515_packet_t p;
struct hmc5883_t hmc;
uint8_t rc;
hmc.x = -1;
hmc.y = -1;
hmc.z = -1;
rc = hmc_read(&hmc);
if (rc) {
rc = mcp2515_send_wait(TYPE_sensor_error, MY_ID, SENSOR_gyro, &rc, sizeof(rc));
return rc;
}
p.type = type;
p.id = ID_9dof;
p.sensor = SENSOR_gyro;
p.data[0] = hmc.x >> 8;
p.data[1] = hmc.x;
p.data[2] = hmc.y >> 8;
p.data[3] = hmc.y;
p.data[4] = hmc.z >> 8;
p.data[5] = hmc.z;
p.len = 6;
return mcp2515_sendpacket_wait(&p);
}
void Magnetometer::testDevice(){
hmc_read(0x00);
Serial.println(res,BIN);
hmc_read(0x01);
Serial.println(res,BIN);
hmc_read(0x02);
Serial.println(res,BIN);
hmc_read(0x09);
Serial.println(res,BIN);
hmc_read(0x0A);
Serial.println(res,BIN);
hmc_read(0x0B);
Serial.println(res,BIN);
hmc_read(0x0C);
Serial.println(res,BIN);
}
