Ian Hua / Quadcopter-mbedRTOS

RTOS-Threads/src/Task4.cpp

Committer:
pHysiX
Date:
2014-04-30
Revision:
5:4879ef0e6d41
Parent:
3:605fbcb54e75
Child:
7:3d28cfc4901b

File content as of revision 5:4879ef0e6d41:

/* Update ESC */
/* 200Hz <= PWM frequency <= 400Hz */

#include "tasks.h"
#include "setup.h"

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

void Task4(void const *argurment)
{
    /* [YAW PITCH ROLL THROTTLE AUX] */
    ESCpower[0] = (RCCommand[3]*9/10) + (pitch_adjust / 2) - (roll_adjust / 2) + (yaw_adjust / 2);
    ESCpower[1] = (RCCommand[3]*9/10) + (pitch_adjust / 2) + (roll_adjust / 2) - (yaw_adjust / 2);
    ESCpower[2] = (RCCommand[3]*9/10) - (pitch_adjust / 2) + (roll_adjust / 2) + (yaw_adjust / 2);
    ESCpower[3] = (RCCommand[3]*9/10) - (pitch_adjust / 2) - (roll_adjust / 2) - (yaw_adjust / 2);
    
    //BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);

    for (int i = 0; i < 4; i++)
        ESC[i].pulsewidth_us(ESCpower[i]);

    //LED[4] = !LED[4];
}