Hi,
I am using the UM6 gyro with an arduino mega to measure x,y and z angles. Before my pid controller can work, i need to reset the gyro values so the x,y and z angles are all x.
Unfortunately, the datasheet was not super helpful for this and i haven't found anyone else's code to do this.
I have attempted to reset the gyro myself, however the gyro remains the same.
The code i have is below.
Header for gyro config (i.e. zeroing the gyro.)
#ifndef IMU_Config_h
#define IMU_Config_h
#include <SoftwareSerial.h>
#include "Arduino.h"
#include "IMU_Registers.h"
#include "IMU_Global_Variables.h"
#include "IMU_Packet.h"
// Function to zero out the bias in the Gyros
// Use on startup WHEN THE IMU IS STATIONARY.
// Takes around 3 seconds.
void Zero_Gyros(){
/*
unsigned char s = 's';
unsigned char n = 'n';
unsigned char p = 'p';
byte pt = (PT_HAS_DATA | PT_ISNT_BATCH);
byte Address = UM6_ZERO_GYROS;
byte data = 0x01;
unsigned int checksum = s+n+p+pt+Address;//+data;
byte checksum1 = checksum >> 8;
byte checksum0 = checksum & 0xFF;
*/
unsigned char s = 's';
unsigned char n = 'n';
unsigned char p = 'p';
byte pt = (char)(PT_HAS_DATA | PT_ISNT_BATCH);
byte Address = UM6_MISC_CONFIG;
byte b3 = 0b00100000;//0x20; //0b00100000;
byte b2 = 0x00;
byte b1 = 0x00;
byte b0 = 0x00;
//byte data = 0x01;
unsigned int checksum = s + n + p + pt + Address + b3 + b2 + b1 + b0; //+data;
byte checksum1 = checksum >> 8;
byte checksum0 = checksum & 0xFF;
while(!Serial1.available()){}
if(Serial1.available()){
Serial.print("Serial Available.");
Serial1.write(s);
Serial1.write(n);
Serial1.write(p);
Serial1.write(pt);
Serial1.write(Address);
Serial1.write(b3);
Serial1.write(b2);
Serial1.write(b1);
Serial1.write(b0);
//Serial1.write(data);
Serial1.write(checksum1);
Serial1.write(checksum0);
}
// Check if serial is available
n = Serial1.available();
// If serial is available, begin reading the output
if (n > 0){
// Read the serial from the imu sensor
c = Serial1.read();
Serial1.write(c);
// switch case for the IMU state
switch(nState){
// The imu data packet begins with three chars, s, n and p in order
// check if these have been recieved.
case STATE_ZERO : // Begin. Look for 's'.
// Start of new packet
// Delete the old packet data
Reset();
if (c == 's'){
nState = STATE_S;
}// end if
else {
nState = STATE_ZERO;
}// end else
break;
case STATE_S : // Have 's'. Look for 'n'.
if (c == 'n'){
nState = STATE_SN;
}// end if
else {
nState = STATE_ZERO;
}// end else
break;
case STATE_SN : // Have 'sn'. Look for 'p'.
if (c == 'p'){
nState = STATE_SNP;
}// end if
else {
nState = STATE_ZERO;
}// end else
break;
// The data packet header has been recieved
// The next output of the IMU is the packet type
case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
UM6_Packet.HasData = 1 && (c & PT_HAS_DATA);
UM6_Packet.IsBatch = 1 && (c & PT_IS_BATCH);
UM6_Packet.BatchLength = ((c >> 2) & 0b00001111);
UM6_Packet.CommFail = 1 && (c & PT_COMM_FAIL);
nState = STATE_PT;
if (UM6_Packet.HasData == PT_HASNT_DATA){
Serial.print("Packet has no data\n");
if (UM6_Packet.IsBatch == PT_ISNT_BATCH){
Serial.print("Packet is not batch\n");
}
}
if (UM6_Packet.CommFail == 1){
Serial.print("Communication failed\n");
}
break;
/*
// Calculate the length of the data selection length (bytes)
if (UM6_Packet.IsBatch){
// If the batch flag is set high, the packet length is 4*batch length
UM6_Packet.DataLength = UM6_Packet.BatchLength * 4;
}// end if
else {
// otherwise the packet length is 4
UM6_Packet.DataLength = 4;
}// end else
break;
*/
// The address of the data is read
// This is used to calculate specific outputs
case STATE_PT : // Have PacketType. Read Address.
UM6_Packet.Address = c;
if (UM6_Packet.Address == UM6_ZERO_GYROS){
Serial.print("Gyro Zero Command Recieved");
}
nState = STATE_CHK1;
//nDataByteCount = 0;
//nState = STATE_READ_DATA;
break;
// Read the data from the IMU
case STATE_READ_DATA : // Read Data. (UM6_PT.BatchLength * 4) bytes.
aPacketData[nDataByteCount] = c;
nDataByteCount++;
if (nDataByteCount >= UM6_Packet.DataLength){
nState = STATE_CHK1;
}// end if
break;
// Two checksums are sent after the data
case STATE_CHK1 : // Read Checksum 1
UM6_Packet.Checksum1 = c;
nState = STATE_CHK0;
break;
case STATE_CHK0 : // Read Checksum 0
UM6_Packet.Checksum0 = c;
nState = STATE_DONE;
break;
case STATE_DONE : // Entire packet consumed. Process packet
//ProcessPacket();
Serial.print("Packet complete\n");
nState = STATE_ZERO;
break;
}// end switch(nstate)
}
}
Any help is greatly appreciated!