Diff: userTask.cpp

Revision:

18:5aa48aec9cae

Parent:

17:f9610f3cfa1b

Child:

20:0394e15412c3

--- a/userTask.cpp	Fri Nov 30 05:24:27 2018 +0000
+++ b/userTask.cpp	Sat Dec 01 14:03:08 2018 +0000
@@ -4,12 +4,30 @@
 #include "queue.h"
 #include "globalFlags.h"
 #include "HbManager.h"
-#include "command.h"
+#include "hbCommand.h"
+#include "uart.h"
+#include "fpga.h"
 
+//タスクハンドル（停止とか再開に必要）
+static xTaskHandle  tskHandle[3]={NULL,};
 
-//----------------------------------------------------------------
+//タスク停止
+//------------------
+static void taskStop(int iId){
+    sp.printf("Task[%d] Stop\r\n" , iId);
+    vTaskSuspend(tskHandle[iId]);//タスクを止める
+}
+
+//タスク再開
+//------------------
+static void taskStart(int iId){
+    sp.printf("Task[%d] Start!!\r\n" , iId);
+    vTaskResume(tskHandle[iId]);//タスク再開
+}
+
+//========================================================
 //ホバーバイク制御タスク
-//----------------------------------------------------------------
+//========================================================
 void taskHbControl(void *pvParameters){
     int8_t          *pcTaskName;
     portTickType    xLastWakeTime;
@@ -18,19 +36,29 @@
     pcTaskName = (int8_t *) pvParameters;
     xLastWakeTime = xTaskGetTickCount();
     
+    //
     while(1){
         led1=!led1;
-        //
-        hb.controlEngine();
+        
+        hb.getAttitude();//現在角度読み出し
+        hb.controlAttitude();//姿勢制御
+        hb.controlMotor();//モーター指令出し
+        hb.controlEngine();//エンジン回転数読み出し
+        
+        
+        //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
+        if(gf_Print.flg!=0){
+            gf_Print.flg=gf_Mon.flg;
+            sp.printf("\r\n");
+        }
         
         //次の周期まで待つ
         vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
     }
 }
-
-//----------------------------------------------------------------
+//========================================================
 //コマンド解析タスク
-//----------------------------------------------------------------
+//========================================================
 void taskCmdParser(void *pvParameters){
     int8_t *pcTaskName;
     portTickType xLastWakeTime;
@@ -38,16 +66,58 @@
     pcTaskName = (int8_t *) pvParameters;
     xLastWakeTime = xTaskGetTickCount();
     
+    static bool runFlg = true;
+    
+    vTaskDelay(300);//制御タスクを先に走らせたいので待たす
+    
     while(1){
         led2=!led2;
-        //
+        //コマンド解釈
         commandParse();
+        
+        if(runFlg){//
+            if(gf_Chk.flg != 0){//チェックフラグが何かたってた
+                sp.printf("Enter debug \r\n");
+                taskStop(0);//制御タスクを止める
+                taskStart(2);//デバッグタスク再開
+                runFlg = false;
+            }
+        }else{
+            if(gf_Chk.flg == 0){
+                sp.printf("Exit debug \r\n");
+                taskStop(2);//デバッグタスクを止める
+                taskStart(0);//制御タスク再開
+                runFlg = true;
+            }
+        }
+        
         //次の周期まで待つ
         vTaskDelayUntil(&xLastWakeTime, 100 / portTICK_RATE_MS );
     }
 }
-
-
+//========================================================
+//デバッグ用タスク
+//========================================================
+void taskDebug(void *pvParameters){
+    int8_t *pcTaskName;
+    portTickType xLastWakeTime;
+    
+    pcTaskName = (int8_t *) pvParameters;
+    xLastWakeTime = xTaskGetTickCount();
+        
+    taskStop(2);
+    
+    while(1){
+        if(gf_Chk.flg != 0){
+            led3= 1;
+            fpgaMotorChk();
+            led3= 0;
+        }
+ 
+        //タスク停止させて次の周期まで待つ
+        taskStop(2);
+    }
+}
 
 //-------------------------------------------------------------
 //初期化：タスク登録
@@ -55,14 +125,19 @@
 void taskInit(){
     portBASE_TYPE   TaskRtn;
     
-    //
-    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"Task50Hz", 192, NULL, 1, NULL);
+    //制御タスク
+    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"Task50Hz", 192, NULL, 2, &tskHandle[0]);
     if(TaskRtn==pdTRUE){printf("Hoverbike Control task Set\r\n");}
-    //
-    TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskParser", 192, NULL, 1, NULL);
+    
+    //コマンド解析タスク
+    TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskParser", 192, NULL, 1, &tskHandle[1]);
     if(TaskRtn==pdTRUE){printf("Command Parser task Set\r\n");}
     
-    //カーネルの起動
+    //デバッグタスク
+    TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
+    if(TaskRtn==pdTRUE){printf("Debug Parser task Set\r\n");}
+    
+    //RTOSカーネルの起動
     vTaskStartScheduler();
 }