|
|
|
/* stick.c */
|
|
|
|
|
|
|
|
#ifdef WE_HAVE_SQRT
|
|
|
|
#include <math.h>
|
|
|
|
#else
|
|
|
|
#include "fisqrt.h"
|
|
|
|
#endif
|
|
|
|
#include "matrix.h"
|
|
|
|
#include "stick.h"
|
|
|
|
#include "dcm.h"
|
|
|
|
#include "uart.h"
|
|
|
|
#include "timer.h"
|
|
|
|
#include "trig.h"
|
|
|
|
#include "motor.h"
|
|
|
|
#include "wmp.h"
|
|
|
|
#include "status.h"
|
|
|
|
#include "watchdog.h"
|
|
|
|
|
|
|
|
#define TWO_PI 6.28318531f
|
|
|
|
#define PI 3.14159265f
|
|
|
|
|
|
|
|
#define MIN_X 15830
|
|
|
|
#define MAX_X 28300
|
|
|
|
#define CENTRE_X 22100
|
|
|
|
|
|
|
|
#define MIN_Y 18530
|
|
|
|
#define MAX_Y 28200
|
|
|
|
#define CENTRE_Y 22100
|
|
|
|
|
|
|
|
#define MIN_Z 15800
|
|
|
|
#define MAX_Z 28304
|
|
|
|
#define CENTRE_Z 22100
|
|
|
|
|
|
|
|
#define MIN_THR 16500
|
|
|
|
#define MAX_THR 28275
|
|
|
|
|
|
|
|
#define MIN_REAL_THR 15830
|
|
|
|
|
|
|
|
#define CENTRE_ZONE 100
|
|
|
|
|
|
|
|
/* Full scale is a roll/pitch angle of 30 degrees from the vertical */
|
|
|
|
#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X))
|
|
|
|
#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y))
|
|
|
|
|
|
|
|
/* Full scale is a complete rotation in one second */
|
|
|
|
#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z))
|
|
|
|
|
|
|
|
/* 0 is min throttle, 1 is max throttle */
|
|
|
|
#define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR))
|
|
|
|
|
|
|
|
float stick_yaw = 0.0f;
|
|
|
|
|
|
|
|
unsigned int stick_counter;
|
|
|
|
|
|
|
|
void stick_update(float x, float y, float z)
|
|
|
|
{
|
|
|
|
float tz = delta_t * z;
|
|
|
|
|
|
|
|
stick_yaw += tz;
|
|
|
|
|
|
|
|
if (stick_yaw <= -PI)
|
|
|
|
stick_yaw += TWO_PI;
|
|
|
|
|
|
|
|
if (stick_yaw > PI)
|
|
|
|
stick_yaw -= TWO_PI;
|
|
|
|
|
|
|
|
#if 0
|
|
|
|
z = stick_yaw;
|
|
|
|
#endif
|
|
|
|
|
|
|
|
x = sine(x);
|
|
|
|
y = sine(y);
|
|
|
|
#if 0
|
|
|
|
z = 1.0/fisqrt(1-x*x-y*y);
|
|
|
|
#endif
|
|
|
|
|
|
|
|
dcm_attitude_error(x, y, z);
|
|
|
|
}
|
|
|
|
|
|
|
|
void stick_input(void) {
|
|
|
|
float x, y, z, throttle;
|
|
|
|
if (timer_allvalid()) {
|
|
|
|
x = timer_input(0);
|
|
|
|
y = timer_input(1);
|
|
|
|
throttle = timer_input(2);
|
|
|
|
z = timer_input(3);
|
|
|
|
|
|
|
|
if (!status_armed()) {
|
|
|
|
if ((throttle < MIN_THR) &&
|
|
|
|
(x > (CENTRE_X - CENTRE_ZONE)) &&
|
|
|
|
(x < (CENTRE_X + CENTRE_ZONE)) &&
|
|
|
|
(y > (CENTRE_Y - CENTRE_ZONE)) &&
|
|
|
|
(y < (CENTRE_Y + CENTRE_ZONE)) &&
|
|
|
|
(z > (CENTRE_Z - CENTRE_ZONE)) &&
|
|
|
|
(z < (CENTRE_Z + CENTRE_ZONE)))
|
|
|
|
status_set_ready(STATUS_MODULE_STICK, TRUE);
|
|
|
|
else
|
|
|
|
status_set_ready(STATUS_MODULE_STICK,FALSE);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
x -= CENTRE_X;
|
|
|
|
y -= CENTRE_Y;
|
|
|
|
z -= CENTRE_Z;
|
|
|
|
x = x * SCALE_X;
|
|
|
|
y = y * SCALE_Y;
|
|
|
|
z = z * SCALE_Z;
|
|
|
|
throttle = (throttle - MIN_THR) * SCALE_THROTTLE;
|
|
|
|
if (throttle < 0.0)
|
|
|
|
throttle = 0.0;
|
|
|
|
} else {
|
|
|
|
x = 0.0f;
|
|
|
|
y = 0.0f;
|
|
|
|
z = 0.0f;
|
|
|
|
throttle = 0.0f;
|
|
|
|
status_set_ready(STATUS_MODULE_STICK,FALSE);
|
|
|
|
}
|
|
|
|
|
|
|
|
motor_set_throttle(throttle);
|
|
|
|
|
|
|
|
watchdog_kick(WATCHDOG_STICK);
|
|
|
|
|
|
|
|
/* So the controls are back to front. Let's fix that. */
|
|
|
|
x = -x;
|
|
|
|
y = -y;
|
|
|
|
z = -z;
|
|
|
|
|
|
|
|
#if 0
|
|
|
|
if ((stick_counter % 100) == 0) {
|
|
|
|
putstr("[");
|
|
|
|
putint_s((int)(z * 10000));
|
|
|
|
putstr("] ");
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#if 1
|
|
|
|
stick_update(x, y, z);
|
|
|
|
#else
|
|
|
|
if ((stick_counter % 100) == 0)
|
|
|
|
stick_update(x, y, z);
|
|
|
|
#endif
|
|
|
|
|
|
|
|
/*
|
|
|
|
if ((stick_counter & 3) == 0)
|
|
|
|
stick_send_packet();
|
|
|
|
*/
|
|
|
|
stick_counter++;
|
|
|
|
}
|
|
|
|
|