void hard_reset(void)
{
W5200_RST_START;
delay_millis(200); // Asks for 150m, 200 to be safe
W5200_RST_STOP;
delay_millis(200); // Asks for 150m, 200 to be safe
}
int main(int argc, char** argv) {
mPORTAClearBits(BIT_6); //Clear bits to ensure the LED is off.
mPORTASetPinsDigitalOut(BIT_6); //Set port as output
while(1)
{
//PORTAbits.RA2 = ~PORTAbits.RA2;
mPORTAToggleBits(BIT_6);
delay_millis(100);
}
return (EXIT_SUCCESS);
}
int w5200_init(void)
{
count = 0;
sock = 0;
fst = &qd[0];
lst = &qd[0];
// Turn everything on
W5200_PWR_ON;
W5200_CS_STOP;
W5200_RST_STOP;
// Give it some time
delay_millis(200);
// Go hard in the paint
hard_reset();
uint8_t vsr = read_VRSN();
while(vsr != 0x03)
{
vsr = read_VRSN();
delay_for_1000_nops_x(8);
}
write_MR(MR_CONF);
write_GAR(gateway_ip);
write_SUBR(w52_const_subnet_classC);
write_SHAR(w52_const_mac_default);
write_SIPR(dest_ip);
write_IMR(IMR_CONF);
write_IMR(IMR2_CONF);
write_IR2(IR2_CONF);
write_PHYST(PHY_CONF);
return init_sockets();
}
void delay_seconds (uint n)
{
while (n --)
delay_millis (1000);
}
void gyro_offset_calibration()
{
uint8_t i;
int16_t prev_gyro[3];
int16_t gyro[3];
float gyro_offset[3];
int8_t tilt_detected = 0;
int16_t gyro_calibration_counter = GYRO_ITERATIONS;
// TODO: Implement following function in spike_328p_i2c
imu_set_dlpf();
// TODO: Possibly implement in a tick process
// wait 2 seconds
//_delay_ms(2000);
//LOG("wait 2 sec...\r\n");
delay_millis(2000);
while(gyro_calibration_counter > 0)
{
if(gyro_calibration_counter == GYRO_ITERATIONS)
{
// TODO: Possibly implement in a tick process
delay_millis(700);
// TODO: Implement following function in spike_328p_i2c
imu_get_rotation(&gyro[0], &gyro[1], &gyro[2]);
for(i=0; i<3; i++)
{
gyro_offset[i] = 0;
prev_gyro[i] = gyro[i];
}
}
//LOG("imu_get_rotation:\r\n");
imu_get_rotation(&gyro[0], &gyro[1], &gyro[2]);
for (i=0; i<3; i++)
{
if(abs(prev_gyro[i] - gyro[i]) > TOL)
{
tilt_detected++;
#ifdef GYRO_DEBUG
LOG("i=%d counter=%d diff=%d gyro[i]=%d, prev_gyro[i]=%d\r\n", i, gyro_calibration_counter, prev_gyro[i] - gyro[i], gyro[i], prev_gyro[i]);
#endif
break;
}
}
for (i=0; i<3; i++)
{
gyro_offset[i] += (float)gyro[i]/GYRO_ITERATIONS;
prev_gyro[i] = gyro[i];
}
gyro_calibration_counter--;
if(tilt_detected >= 1)
{
LOG("gyro calibration failed, retrying...\r\n");
gyro_calibration_counter = GYRO_ITERATIONS;
tilt_detected = 0;
}
}
// Put results into integer
config.gyro_offset_x = (int16_t) gyro_offset[0];
config.gyro_offset_y = (int16_t) gyro_offset[1];
config.gyro_offset_z = (int16_t) gyro_offset[2];
LOG("updating gyro calibration offsets...\r\n");
LOG("config.gyro_offset[x,y,z]: %ld\t%ld\t%ld\r\n", config.gyro_offset_x, config.gyro_offset_y, config.gyro_offset_z);
// TODO: Review
imu_init();
}
uint8_t accl_calibration()
{
int16_t dev_val[3];
int16_t min_acc[3] = {INT_MAX, INT_MAX, INT_MAX};
int16_t max_acc[3] = {INT_MIN, INT_MIN, INT_MIN};
float acc_offset[3] = {0,};
delay_millis(500); // delay 0.5 seconds
for (uint8_t i=0; i<ACC_ITERATIONS; i++){
imu_get_acceleration(&dev_val[0], &dev_val[1], &dev_val[2]);
for (uint8_t j=0; j<3; j++){
acc_offset[j] += (float) dev_val[j]/ACC_ITERATIONS;
if(dev_val[j] > max_acc[j]){
max_acc[j] = dev_val[j];
}
if(dev_val[j] < min_acc[j]){
min_acc[j] = dev_val[j];
}
}
delay_millis(2); // 2ms
}
// Debugging
#ifdef GYRO_DEBUG
for(uint8_t j=0; j<3; j++) {
LOG("gyro: avg/max/min[");
LOG("%d", (uint8_t)j);
LOG(("] "));
LOG("%d", acc_offset[j], 3);
LOG((" / "));
LOG("%d", max_acc[j]);
LOG((" / "));
LOG("%d", min_acc[j]);
LOG("\r\n");
}
#endif
for (uint8_t i=0; i<3; i++){
if((max_acc[i] - min_acc[i]) > ACC_THRESH_FAIL){
return -1; // failed
}
}
// store calibration
if(abs(acc_offset[0]) < ACC_THRESH_GMIN){
config.acc_offset_x = acc_offset[0];
}
if(abs(acc_offset[1]) < ACC_THRESH_GMIN){
config.acc_offset_y = acc_offset[1];
}
if(abs(acc_offset[2]) < ACC_THRESH_GMIN){
config.acc_offset_z = acc_offset[2];
}
return 0;
}
