controllers.c

/*
 * controllers.c
 *
 *  Created on: Oct 11, 2014
 *      Author: ucart
 */

/**
 * Lots of useful information in controllers.h, look in there first
 */
#include "controllers.h"
#include "iic_utils.h"
#include "quadposition.h"
#include "util.h"
#include "uart.h"
#include "sleep.h"
#include "stdio.h"
#include <math.h>

// 0 was -6600
//int motor0_bias = -4500, motor1_bias = 100, motor2_bias = 5300, motor3_bias = 10300;
int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;

/**
 * Takes the raw signal inputs from the receiver and filters it so the
 * quadcopter doesn't flip or do something extreme
 */
void filter_PWMs(int* mixer) {

}

/**
 * Converts PWM signals into 4 channel pitch, roll, yaw, throttle
 */
// javey: unused
void PWMS_to_Aero(int* PWMs, int* aero) {
	/**
	 * Reference used to derive equations
	 */
	//	pwm0 = throttle_base - pitch_base + yaw_base;
	//	pwm1 = throttle_base + roll_base - yaw_base;
	//	pwm2 = throttle_base - roll_base - yaw_base;
	//	pwm3 = throttle_base + pitch_base + yaw_base;

	aero[THROTTLE] = (PWMs[0] + PWMs[1] + PWMs[2] + PWMs[3]) / 4;
	aero[ROLL]     = (PWMs[1] - PWMs[2]) / 2;
	aero[PITCH]    = (PWMs[3] - PWMs[0]) / 2;
	aero[YAW]      = (PWMs[3] + PWMs[0] - PWMs[1] - PWMs[2]) / 4;
}