Hi all,
this is my first post in this forum.
I've trying to adapt an Arduino sketch from SatNOGS project to support a LSM303 magnetometer/accelerometer sensor to positioning it at zero at start.
I've a GY-511 chip (LSM303DLHC) and I'm using the Pololu library to get the values. Firstly I calibrated the sensor and I got the max and min values.
Next I write a simple function to get the heading, pitch and roll values compensated. I've tested the function and it works fine and the values I'm getting are fine.
But when I use it from the rotator sketch I always get "LSM303 could not be initialized" when I call:
lsm303.init(LSM303::device_auto, LSM303::sa0_auto) returns false when I call from the main sketch.
This is the code to get the sensor values "orientationTilt.h":
#include <LSM303.h>
LSM303 lsm303;
// Min and max values obtained from calibration for tilt compensated heading: see Calibration.ino example from LSM303 library
#define TILT_MIN_X -530
#define TILT_MIN_Y -539
#define TILT_MIN_Z -475
#define TILT_MAX_X +610
#define TILT_MAX_Y +430
#define TILT_MAX_Z +450
/*
In our case note that:
heading = azimuth
pitch = elevation
*/
struct orientation {
float pitch;
float roll;
float heading;
};
struct orientation getOrientationTilt(){
const float alpha = 0.10; // Alpha
float fXa, fYa, fZa = 0; // Low-pass filtered accelerometer variables
if (lsm303.init(LSM303::device_auto, LSM303::sa0_auto)) {
lsm303.enableDefault();
/*
Calibration values: the default values of +/-32767 for each axis lead to an assumed
magnetometer bias of 0. Use the Pololu LSM303 library Calibrate example program to
determine appropriate values for each LSM303 sensor.
*/
lsm303.m_min = (LSM303::vector<int16_t>) {TILT_MIN_X, TILT_MIN_Y, TILT_MIN_Z};
lsm303.m_max = (LSM303::vector<int16_t>) {TILT_MAX_X, TILT_MAX_Y, TILT_MAX_Z};
// Apply low-pass filter to accelerometer data
for (byte i = 0; i < 30; i++) {
lsm303.read(); // Read accelerometer and magnetometer data
fXa = lsm303.a.x * alpha + (fXa * (1.0 - alpha));
fYa = lsm303.a.y * alpha + (fYa * (1.0 - alpha));
fZa = lsm303.a.z * alpha + (fZa * (1.0 - alpha));
}
// Calculate orientation data
float pitch = (atan2(fXa, sqrt((long)sq(fYa) + (long)sq(fZa))) * 180) / PI; // horizontal=0º -> up=+90º -> horizontal=0º -> down=-90º -> horizontal=0º
float roll = (atan2(fYa, fZa) * 180) / PI;
float heading = lsm303.heading((LSM303::vector<int>) {1, 0, 0}); // PCB orientation: {1,0,0}=+X, {0,1,0}=+Y, ...
//Return orientation
orientation retOrientation;
retOrientation.pitch = pitch;
retOrientation.roll = roll;
retOrientation.heading = heading;
return retOrientation;
}
else {
Serial.println(F("Error: Could not initialize LSM303!"));
}
}
And next an excerpt of the main sketch:
#define SAMPLE_TIME 0.1 ///< Control loop in s
#define RATIO 4 ///< Gear ratio of rotator gear box default 54
#define MICROSTEP 32 ///< Set Microstep
#define MIN_PULSE_WIDTH 1.9 ///< In microsecond for AccelStepper
#define MAX_SPEED 1000 ///< In steps/s, consider the microstep
#define MAX_ACCELERATION 800 ///< In steps/s^2, consider the microstep
#define SPR 6400 ///< Step Per Revolution, consider the microstep
#define MIN_M1_ANGLE 0 ///< Minimum angle of azimuth
#define MAX_M1_ANGLE 360 ///< Maximum angle of azimuth
#define MIN_M2_ANGLE 0 ///< Minimum angle of elevation
#define MAX_M2_ANGLE 90 ///< Maximum angle of elevation
#define DEFAULT_HOME_STATE LOW ///< Change to LOW according to Home sensor
#define HOME_DELAY 12000 ///< Time for homing Deceleration in millisecond
#include <AccelStepper.h>
#include <Wire.h>
#include "globals.h"
#include "easycomm.h"
#include "rotator_pins.h"
//#include <rs485.h>
#include "endstop.h"
//#include <watchdog.h>
#include "orientationTilt.h"
uint32_t t_run = 0; // run time of uC
easycomm comm;
AccelStepper stepper_az(1, M1IN1, M1IN2);
AccelStepper stepper_el(1, M2IN1, M2IN2);
endstop switch_az(SW1, DEFAULT_HOME_STATE), switch_el(SW2, DEFAULT_HOME_STATE);
orientation compass;
//wdt_timer wdt;
enum _rotator_error homing(int32_t seek_az, int32_t seek_el);
int32_t deg2step(float deg);
float step2deg(int32_t step);
void setup() {
// Homing switch
switch_az.init();
switch_el.init();
// Serial Communication
comm.easycomm_init();
// Init sensors
//initSensors();
// Stepper Motor setup
stepper_az.setEnablePin(MOTOR_EN);
stepper_az.setPinsInverted(false, false, true);
stepper_az.enableOutputs();
stepper_az.setMaxSpeed(MAX_SPEED);
stepper_az.setAcceleration(MAX_ACCELERATION);
stepper_az.setMinPulseWidth(MIN_PULSE_WIDTH);
stepper_el.setPinsInverted(false, false, true);
stepper_el.enableOutputs();
stepper_el.setMaxSpeed(MAX_SPEED);
stepper_el.setAcceleration(MAX_ACCELERATION);
stepper_el.setMinPulseWidth(MIN_PULSE_WIDTH);
// Initialize WDT
// wdt.watchdog_init();
**compass = getOrientationTilt(); <-- function fails here**
stepper_az.setCurrentPosition(deg2step(compass.heading));
stepper_el.setCurrentPosition(deg2step(compass.pitch));
}
void loop() { ...... }
I think there's a conflict with the Serial but I don't know how to solve it.
Thanks in advance.