Ian Hua / Quadcopter-mbedRTOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

RTOS-Threads/src/Task4.cpp

Committer:
pHysiX
Date:
2014-05-12
Revision:
30:d9b988f8d84f
Parent:
27:18b6580eb0b1
Child:
31:3dde2201e54d

File content as of revision 30:d9b988f8d84f:

/* 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:    200Hz
 * 200Hz <= PWM frequency <= 400Hz
 * Refer to tasks.h to change PWM frequency
 * Timing:      1500us
 */
#include "tasks.h"
#include "setup.h"

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

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

#ifdef TIME_TASK4
Timer _t4;
#endif
void Task4(void const *argurment)
{
#ifdef TIME_TASK4
    _t4.reset();
    _t4.start();
#endif
    if (armed) {
        if (calibration_mode) {
            for (int i = 0; i < 4; i++)
                ESC[i].pulsewidth_us(RCCommand[3]);
        } else if (RCCommand[3] > 1150) {
            if (counterESC) {
                for (int i = 0; i < 3; i++)
                    adjust[i] /= 2.0;

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

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

                counterESC = false;
            } else {
                stallESC++;

                if (stallESC > 1) {
                    imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
                    stallESC = 0;
                }
            }
        } else {
            for (int i = 0; i < 4; i++) {
                ESCpower[i] = 980;
                ESC[i].pulsewidth_us(ESCpower[i]);
            }
        }
    } else {
        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] = 980;
            ESC[i].pulsewidth_us(ESCpower[i]);
        }
    }

    if (ESC_check)
        BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);

#ifdef TIME_TASK4
    _t4.stop();
    BT.printf("%d\n", _t4.read_us());
#endif
}

int constrainESC(float input)
{
    if (input < 1150.0)
        return 1150;
    else if (input > 2000.0)
        return 2000;
    else
        return (int) input;
}