You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
234 lines
4.7 KiB
234 lines
4.7 KiB
11 years ago
|
/* sensors.c */
|
||
|
|
||
|
#include "mpu6050.h"
|
||
|
#include "dcm.h"
|
||
|
#include "fisqrt.h"
|
||
|
#include "watchdog.h"
|
||
|
#include "status.h"
|
||
|
#include "abs.h"
|
||
|
#include "panic.h"
|
||
|
#include "uart.h"
|
||
|
#include "log.h"
|
||
|
#include "stick.h"
|
||
|
|
||
|
bool (*sensor_start_fns[])(void) = {
|
||
|
mpu6050_start_sample,
|
||
|
};
|
||
|
|
||
|
#define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0]))
|
||
|
|
||
|
static unsigned int next_sensor;
|
||
|
|
||
|
static bool sensors_zero;
|
||
|
static bool sensors_update;
|
||
|
static unsigned int sensors_discard;
|
||
|
static unsigned int sensors_generation;
|
||
|
|
||
|
float sensors_gyro_roll;
|
||
|
float sensors_gyro_pitch;
|
||
|
float sensors_gyro_yaw;
|
||
|
|
||
|
float sensors_temp;
|
||
|
|
||
|
float sensors_accel_x;
|
||
|
float sensors_accel_y;
|
||
|
float sensors_accel_z;
|
||
|
|
||
|
float gyro_yaw_zero;
|
||
|
float gyro_pitch_zero;
|
||
|
float gyro_roll_zero;
|
||
|
|
||
|
void sensors_write_log(void);
|
||
|
void sensors_process(void);
|
||
|
|
||
|
#define TWO_PI 6.28318531f
|
||
|
#define DEG_TO_RAD (TWO_PI/360.0f)
|
||
|
|
||
|
/* The gyro has to stay within this limit in each axis in order to arm */
|
||
|
#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD)
|
||
|
|
||
|
#define GYRO_ZERO_COUNT 1000
|
||
|
|
||
|
void sensors_dump(void);
|
||
|
|
||
|
bool sensors_init(void)
|
||
|
{
|
||
|
next_sensor = 0;
|
||
|
|
||
|
if (!mpu6050_init())
|
||
|
return FALSE;
|
||
|
|
||
|
return TRUE;
|
||
|
}
|
||
|
|
||
|
bool sensors_next_sample(void)
|
||
|
{
|
||
|
bool result;
|
||
|
|
||
|
result = (sensor_start_fns[next_sensor])();
|
||
|
if (next_sensor >= SENSOR_START_FNS)
|
||
|
next_sensor = 0;
|
||
|
|
||
|
return result;
|
||
|
}
|
||
|
|
||
|
void sensors_sample_done(void)
|
||
|
{
|
||
|
if (next_sensor == 0) {
|
||
|
sensors_write_log();
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
if (!sensors_next_sample())
|
||
|
panic(PANIC_SENSOR_FAIL);
|
||
|
}
|
||
|
|
||
|
bool sensors_start_sample(void)
|
||
|
{
|
||
|
next_sensor = 0;
|
||
|
return sensors_next_sample();
|
||
|
}
|
||
|
|
||
|
void sensors_write_gyro_data(float roll, float pitch, float yaw)
|
||
|
{
|
||
|
#if 0
|
||
|
sensors_gyro_roll = roll - gyro_roll_zero;
|
||
|
sensors_gyro_pitch = pitch - gyro_pitch_zero;
|
||
|
sensors_gyro_yaw = yaw - gyro_yaw_zero;
|
||
|
#else
|
||
|
sensors_gyro_roll = roll;
|
||
|
sensors_gyro_pitch = pitch;
|
||
|
sensors_gyro_yaw = yaw;
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
void sensors_write_accel_data(float x, float y, float z)
|
||
|
{
|
||
|
sensors_accel_x = x;
|
||
|
sensors_accel_y = y;
|
||
|
sensors_accel_z = z;
|
||
|
}
|
||
|
|
||
|
void sensors_write_temp_data(float temp)
|
||
|
{
|
||
|
sensors_temp = temp;
|
||
|
/* XXX HACK find a better place for this call */
|
||
|
sensors_process();
|
||
|
}
|
||
|
|
||
|
#define LOG_SIGNATURE_SENSORS 0xDA7ADA7A
|
||
|
#define LOG_SIGNATURE_SENSORS2 0xDA7AF173
|
||
|
void sensors_write_log(void)
|
||
|
{
|
||
|
#if 0
|
||
|
log_put_uint(LOG_SIGNATURE_SENSORS);
|
||
|
log_put_float(sensors_accel_x);
|
||
|
log_put_float(sensors_accel_y);
|
||
|
log_put_float(sensors_accel_z);
|
||
|
log_put_float(sensors_gyro_roll);
|
||
|
log_put_float(sensors_gyro_pitch);
|
||
|
log_put_float(sensors_gyro_yaw);
|
||
|
log_put_float(sensors_temp);
|
||
|
#else
|
||
|
/* XXX this just about comes out in the right place, but by luck */
|
||
|
log_put_uint(LOG_SIGNATURE_SENSORS2);
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
void sensors_start_zero(void)
|
||
|
{
|
||
|
sensors_zero = TRUE;
|
||
|
sensors_update = FALSE;
|
||
|
sensors_discard = 100;
|
||
|
sensors_generation = 0;
|
||
|
putstr("Starting zero\r\n");
|
||
|
mpu6050_start_zero();
|
||
|
}
|
||
|
|
||
|
void sensors_process(void)
|
||
|
{
|
||
|
if (sensors_update) {
|
||
|
#if 1
|
||
|
dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll,
|
||
|
-sensors_gyro_yaw);
|
||
|
#else
|
||
|
dcm_update(0.0, 0.0, 0.0);
|
||
|
#endif
|
||
|
if (!status_armed()) {
|
||
|
if ( (abs(sensors_gyro_roll) < GYRO_RATE_THRESHOLD) &&
|
||
|
(abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) &&
|
||
|
(abs(sensors_gyro_yaw) < GYRO_RATE_THRESHOLD)) {
|
||
|
status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE);
|
||
|
} else {
|
||
|
status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE);
|
||
|
}
|
||
|
|
||
|
}
|
||
|
|
||
|
sensors_generation++;
|
||
|
|
||
|
#if SEND_DCM
|
||
|
if ((sensors_generation % 40) == 0) {
|
||
|
dcm_send_packet();
|
||
|
sensors_dump();
|
||
|
}
|
||
|
#endif
|
||
|
|
||
|
} else if (sensors_zero) {
|
||
|
if (sensors_discard) {
|
||
|
sensors_discard--;
|
||
|
} else {
|
||
|
gyro_yaw_zero += sensors_gyro_yaw;
|
||
|
gyro_pitch_zero += sensors_gyro_pitch;
|
||
|
gyro_roll_zero += sensors_gyro_roll;
|
||
|
sensors_generation++;
|
||
|
if (sensors_generation >= GYRO_ZERO_COUNT) {
|
||
|
sensors_zero = FALSE;
|
||
|
sensors_update = TRUE;
|
||
|
sensors_generation = 0;
|
||
|
gyro_yaw_zero /= GYRO_ZERO_COUNT;
|
||
|
gyro_pitch_zero /= GYRO_ZERO_COUNT;
|
||
|
gyro_roll_zero /= GYRO_ZERO_COUNT;
|
||
|
putstr("Zero finished\r\n");
|
||
|
status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
watchdog_kick(WATCHDOG_GYRO);
|
||
|
|
||
|
#if 1
|
||
|
dcm_drift_correction(-sensors_accel_y, -sensors_accel_x,
|
||
|
-sensors_accel_z);
|
||
|
#endif
|
||
|
#if 0
|
||
|
dcm_drift_correction(sensors_accel_x, sensors_accel_y,
|
||
|
sensors_accel_z);
|
||
|
#endif
|
||
|
|
||
|
watchdog_kick(WATCHDOG_ACCEL);
|
||
|
stick_input();
|
||
|
}
|
||
|
|
||
|
void sensors_dump(void)
|
||
|
{
|
||
|
putstr("(");
|
||
|
putint_s((int)(sensors_accel_x * 1000.0));
|
||
|
putstr(",");
|
||
|
putint_s((int)(sensors_accel_y * 1000.0));
|
||
|
putstr(",");
|
||
|
putint_s((int)(sensors_accel_z * 1000.0));
|
||
|
putstr(")");
|
||
|
|
||
|
putstr("(");
|
||
|
putint_s((int)(sensors_gyro_roll * 1000.0));
|
||
|
putstr(",");
|
||
|
putint_s((int)(sensors_gyro_pitch * 1000.0));
|
||
|
putstr(",");
|
||
|
putint_s((int)(sensors_gyro_yaw * 1000.0));
|
||
|
putstr(")");
|
||
|
|
||
|
putstr("(");
|
||
|
putint_s((int)(sensors_temp * 1000.0));
|
||
|
putstr(")\r\n");
|
||
|
}
|