File content as of revision 54:a36d39a90c21:

/* File:        Task4.h
 * Author:      Trung Tin Ian HUA
 * Date:        May 2014
 * Purpose:     Thread4: ESC pulsewidth update. Note this is INDEPENDENT of the pulse frequency.
 * Settings:    400Hz
 * 200Hz <= PWM frequency <= 400Hz
 * Frequency:   400Hz
 * Refer to tasks.h to change PWM frequency
 * Timing:
 */
#include "tasks.h"
#include "setup.h"

Semaphore sem_Task4(1);

volatile int ESCpower[4] = {0, 0, 0, 0};
int stallESC = 0;

bool armed = false;
bool ESC_check = false;
bool calibration_mode = false;




// ==================
// === ESC UPDATE ===
// ==================
void Task4(void const *argurment)
{
    while (1) {
        if (calibration_mode) {
            if (armed)
                for (int i = 0; i < 4; i++)
                    ESC[i].pulsewidth_us(RCCommand[3]);
        } else if (!armed) {
            if (ESC_check) {
                BT.printf("Need to ARM to check ESC output!\n");
                ESC_check = false;
            }

            for (int i = 0; i < 4; i++) {
                ESCpower[i] = 990;
                ESC[i].pulsewidth_us(ESCpower[i]);
            }
        } else if (armed) {
            if (RCCommand[3] < 1100) {
                for (int i = 0; i < 4; i++) {
                    ESCpower[i] = 1000;
                    ESC[i].pulsewidth_us(ESCpower[i]);
                }
            } else {
                //PC.printf("T4\n");
                sem_Task4.wait();
                //PC.printf("T4 Sem\n");

                for (int i = 0; i < 3; i++)
                    adjust[i] /= 2.0;

                int throttle = RCCommand[3] * 9/10;

                ESCpower[0] = constrainESC(throttle + (adjust[1]) + (adjust[2]) - adjust[0]);
                ESCpower[1] = constrainESC(throttle + (adjust[1]) - (adjust[2]) + adjust[0]);
                ESCpower[2] = constrainESC(throttle - (adjust[1]) - (adjust[2]) - adjust[0]);
                ESCpower[3] = constrainESC(throttle - (adjust[1]) + (adjust[2]) + adjust[0]);

                for (int i = 0; i < 4; i++)
                    ESC[i].pulsewidth_us(ESCpower[i]);
            } // else
        } //else if (armed)
        Thread::wait(TASK4_PERIOD);
    } //while(1)
} //Task4




// ************************
// *** Helper functions ***
// ************************
int constrainESC(float input)
{
    if (input < 1100.0)
        return 1100;
    else if (input > 2000.0)
        return 2000;
    else
        return (int) input;
}