/**
* \brief Displays light level on LED.
* Used for self-test.
*
* Press button to exit the test
*/
void BrightnessLevel()
{
uint8_t level;
uint8_t button;
do
{
button = ButtonCheck();
level = GetLight();
level /= 32; //convert to 8 levels
/* This displays digit from 0 to 7
ledPutc('0'+level); */
/* This is a test code to display ADC value on LED instead of bar graph
* Don't forget to comment out "level /= 32;" line
sprintf(g_TextBuffer," %d", level);
g_TextBufferLen = strlen( g_TextBuffer );
ShowText();
*/
ledPutc(SYMBOL_LIGHT_LEVEL + 7-level);
_delay_ms( 20 );
} while( button != BUTTON_DOWN );
}
/**
* Display text stored in EEPROM memory using current font.
*
* \param szText Text to display in EEPROM. Must be terminated by 0 or 0xFF.
*
* \retval BUTTON_SHORT Button pressed short
* \retval BUTTON_LONG Button pressed long
* \retval BUTTON_NONE Timeout
*/
uint8_t ledPuts_EE( const uint8_t* szText )
{
int offset = 0;
uint8_t key;
//copy data from EEPROM
for(g_TextBufferLen=0;g_TextBufferLen<TEXTBUFFER_SIZE;g_TextBufferLen++)
{
g_TextBuffer[g_TextBufferLen] = eeprom_read_byte( szText + g_TextBufferLen ); //read byte from eeprom
if( 0 == g_TextBuffer[g_TextBufferLen] || 0xFF /*EEPROM not programmed*/ == g_TextBuffer[g_TextBufferLen] )
{
break; //end of string
}
}
//scroll once entire text
do
{
ScrollLeft(g_TextBuffer, g_TextBufferLen, &offset );
key = ButtonCheck();
if( key == BUTTON_SHORT || key == BUTTON_LONG )
{
return key;
}
_delay_ms(20);
} while( offset );
return BUTTON_NONE;
}
void updateSetpoints(short* yawSetpoint, short* heightSetpoint)
{
// Yaw
if (ButtonCheck(LEFT))
{
*yawSetpoint -= 15;
}
else if (ButtonCheck(RIGHT))
{
*yawSetpoint += 15;
}
if (*yawSetpoint > 300)
{
*yawSetpoint = 300;
}
else if (*yawSetpoint < -300)
{
*yawSetpoint = -300;
}
// Height
if (ButtonCheck(DOWN) )//&& *heightSetpoint > 0)
{
*heightSetpoint -= 10;
}
else if (ButtonCheck(UP))
{
*heightSetpoint += 10;
}
if (*heightSetpoint > 100)
{
*heightSetpoint = 100;
}
else if (*heightSetpoint < 0)
{
*heightSetpoint = 0;
}
return;
}
///////////////////////////////////////////////////////////////////////////////
//////////////////////////////// MAIN ///////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
int main(void)
{
IntMasterDisable();
//=====SYSTEM PERIPHERAL INITIALIZATION=====
// Initialize real time clock
mRTCInit(RTC_RATE_HZ);
// Millisecond timekeeping variable
int time;
//Add periodic tasks to be executed by systick
mRTCAddTask(heightSample, HEIGHT_SAMPLE_RATE_HZ);
// Set up display
mDisplayInit(1000000);
mDisplayLine(" Waiting... ", 0, 5, 15);
// Set up buttons
ButtonInit();
// Set up PWM
mPWMInit();
mPWMEnable(PWM4, false); // tail rotor, yaw control.
mPWMEnable(PWM1, false); // main rotor, height control.
//=========CONTROL INITIALIZATION========
//-----altitude PID control------
// Initialize altitude module
heightInit();
// =========PID parameters========================
//Phils
float H_kp = 1;float H_ki = 1.5;float H_kd = -0.5;
short heightPIDOffset = 40;
float Y_kp = 0.6;float Y_ki = 0;float Y_kd = 0;
short YawPIDOffset = 0;//50;
// Windup regulator
float H_windup_limit = 10;
if (H_ki)
H_windup_limit /= H_ki;
// height PID controller
pid_t heightPID;
PIDInit(&heightPID);
PIDSet(&heightPID, H_kp, H_ki, H_kd, H_windup_limit); // set PID constants
// height PID control variables
//35; // PID out offset such that the rotor is at near-takoff speed
short height = 0;
short heightSetpoint = 0; // in degrees
short heightError = 0;
short heightPIDOut = 0;
//-----yaw PID control-------
// Initialise Yaw decoder module
yawInit(); // yaw monitor
float Y_windup_limit = 20; // Maximum integral contribution to PID output value
if (Y_ki)
Y_windup_limit /= Y_ki; // devide by Y_ki to find maximum value in terms of error
// Yaw PID controller
pid_t yawPID;
PIDInit(&yawPID);
PIDSet(&yawPID, Y_kp, Y_ki, Y_kd, Y_windup_limit); // set PID constants
// yaw PID control variables
short yaw = 0;
short yawSetpoint = 0;
short yawError = 0;
short yawPIDOut = 0;
//
// Enable interrupts to the processor.
IntMasterEnable();
short takeOffFlag = false;
while (!takeOffFlag)
{
if (ButtonCheck(SELECT))
//if (GPIOPinRead(GPIO_PORTG_BASE, 0x80))
{
takeOffFlag = true;
}
}
// Reset setpoints to current position
heightSetpoint = heightGet();
yawSetpoint = yawGetAngle();
mPWMEnable(PWM4, true); // tail rotor, yaw control.
mPWMEnable(PWM1, true); // main rotor, height control.
//spin up routine
//spinUp(heightPIDOffset, yawPID);
// Reset clock to zero for effective helicopter launch time
mRTCSet(0);
while (1)
{
//mDisplayClear();
time = mRTCGetMilliSeconds();
// Update Setpoints
updateSetpoints(&yawSetpoint, &heightSetpoint);
// ==================PID Control=================
if ((time % (RTC_RATE_HZ/PID_RATE_HZ)) /*1000/(float)PID_RATE_HZ*/ == 0)
{
//
// ~~~~~~~~~~~~~~~~~ HEIGHT PID ~~~~~~~~~~~~~~~~
height = heightGet();
heightError = heightSetpoint - height;
heightPIDOut = PIDUpdate(&heightPID, heightError, 1.00 / (float)PID_RATE_HZ) + heightPIDOffset;
if (heightPIDOut > 79)
//heightPIDOut = 79;
if (heightPIDOut < 2)
heightPIDOut = 2;
mPWMSet(PWM1, (unsigned short)heightPIDOut);
//
// ~~~~~~~~~~~~~~~~~~ YAW PID ~~~~~~~~~~~~~~~~~~~
yaw = yawGetAngle();
yawError = yaw - yawSetpoint;
yawPIDOut = PIDUpdate(&yawPID, yawError, 1.00 / (float)PID_RATE_HZ) + YawPIDOffset;
if (yawPIDOut > 79)
yawPIDOut = 79;
if (yawPIDOut < 2)
yawPIDOut = 2;
mPWMSet(PWM4, (unsigned short)yawPIDOut);
// ===============================================
}
// RTC_RATE_HZ - PID_RATE_HZ
if (( (time) % 10) == 0)
{
mDisplayLine("time:%.6d mS", time, 0, 7);
mDisplayLine("Yaw = %.4d' ", (int)yaw, 1, 15);
mDisplayLine("YSet = %.4d' ", (int)yawSetpoint, 2, 15);
mDisplayLine("YErr = %.4d' ", (int)yawError, 3, 15);
mDisplayLine("YOut = %.4d' ", (int)yawPIDOut, 4, 15);
mDisplayLine("height = ~%.3d ", (int)height, 5, 15);
mDisplayLine("Hset = %.4d ", (int)heightSetpoint, 6, 15);
mDisplayLine("Herr = %.4d ", (int)heightError, 7, 15);
mDisplayLine("Hout = %.4d ", (int)heightPIDOut, 8, 15);
}
// should put this as part of the main while loop condition
//if (ButtonCheck(SELECT))
//{
// spinDown();
//}
}
}
void ButtonGuardThread(void* pvParameters)
{
unsigned long ulTick = 0;
s_ulIdleTick = 0;
unsigned long ulLongKeepOff = 0;
unsigned long ulChrgTick = 0;
bool bEnableRunning = true;
while(1) {
ulTick++;
// 按钮状态检测
int nBtnAction = ButtonCheck();
if (nBtnAction != BTN_NOACTION) {
s_ulIdleTick = 0;
if ( (nBtnAction==BTN_PUSHDOWN) && g_bRunning ) {
g_bRunning = false;
MotorRun(g_bRunning);
bEnableRunning = false;
}
else if ( (nBtnAction==BTN_POPUP) && !g_bRunning ) {
if (!bEnableRunning) {
bEnableRunning = true;
}
else {
if (g_bRunable) { // 如果不在SimpleLink操作阶段则不能运行探头
g_bRunning = true;
MotorRun(g_bRunning);
}
}
}
ulLongKeepOff = 0; // 长按关机计数
} else { // 长按关机状态下关闭电源
if ( s_nStatOut == BTN_UP) {
ulLongKeepOff = 0;
} else {
++ulLongKeepOff;
if (ulLongKeepOff/100 > 8) { // 长按8秒关闭电源
MotorRun(false);
TurnOff();
}
}
// 注：
// 长按关机是为了避免运输工程中，如果探头的按钮受到挤压而开机；
// 在这种状态下，按钮控制芯片并不会在一定时间后主动关闭电源，从而
// 会导致电源耗尽或者其他潜在风险。
// 故而解决的方法是检测按钮是否长时间（8S）处于未知或者按下
// 状态，一旦检测到则关闭电源。
// 吴文斌
// 2015 - 3 - 30
}
// 充电状态检测
if (IsCharging()) {
ulChrgTick++;
if (ulChrgTick > 20) {
TRACE("Begin Charging...\n\r");
g_bRunning = false;
MotorRun(g_bRunning);
TurnOff();
}
}
else {
ulChrgTick = 0;
}
// 待机时间检测
++s_ulIdleTick;
if ( (s_ulIdleTick/100/60 > 10) // 连续运行10分钟则停止运行
&& g_bRunning )
{
TRACE("10min to Stop Running...\n\r");
g_bRunning = !g_bRunning;
MotorRun(g_bRunning);
}
if ( s_ulIdleTick/100/60 > 15) { // 待机时间超过15分钟则关闭电源
TRACE("15min to Power Off...\n\r");
TurnOff();
}
vTaskDelay(10/portTICK_PERIOD_MS);
}
}
