Quadrotor from scratch
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.

193 lines
3.5 KiB

/* 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 "status.h"
#include "watchdog.h"
#include "log.h"
#define TWO_PI 6.28318531f
#define PI 3.14159265f
#define MIN_X 8720
#define MAX_X 23800
#define CENTRE_X 16260
#define MIN_Y 8720
#define MAX_Y 23800
#define CENTRE_Y 16260
#define MIN_Z 8720
#define MAX_Z 23800
#define CENTRE_Z 16300
#define MIN_THR 9720
#define MAX_THR 23750
#define MIN_REAL_THR 8720
#define CENTRE_ZONE 100
/* With new TX firmware, CPPM:
* x y thr z
* centre: 16260, 16258, 16000, 16300
* min: 8720, 8718, 8720, 8722
* max: 23790, 23817, 23750, 23803
*/
/* 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);
}
#ifdef STICK_DEBUG_CALIBRATE
void stick_debug_calibrate()
{
unsigned int t1 = timer_input(0);
unsigned int t2 = timer_input(1);
unsigned int t3 = timer_input(2);
unsigned int t4 = timer_input(3);
putstr("S:(");
putint(t1);
putstr(",");
putint(t2);
putstr(",");
putint(t3);
putstr(",");
putint(t4);
putstr(")\r\n");
}
#endif
void stick_input(void) {
float x, y, z, throttle;
unsigned int xi, yi, zi, throttlei;
if (timer_allvalid()) {
xi = timer_input(0);
yi = timer_input(1);
throttlei = timer_input(2);
zi = timer_input(3);
log_put_uint16(xi);
log_put_uint16(yi);
log_put_uint16(throttlei);
log_put_uint16(zi);
x = xi;
y = yi;
throttle = throttlei;
z = zi;
#ifdef STICK_DEBUG_CALIBRATE
if ((stick_counter % 100) == 0)
stick_debug_calibrate();
#endif
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++;
}