rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6126 from mikeller/fix_pid_loop_overloading 

Removed tasks from PID loop that don't belong there.
This commit is contained in:
Michael Keller 2018-06-17 20:08:56 +12:00 committed by GitHub
parent a7fae6b8f0 f412e17e89
commit 2fe9d79903
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 133 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/main/blackbox/blackbox.c
View File

@ -1669,16 +1669,15 @@ void blackboxUpdate(timeUs_t currentTimeUs)
#ifdef USE_FLASHFS
if (blackboxState != BLACKBOX_STATE_ERASING
&& blackboxState != BLACKBOX_STATE_START_ERASE
&& blackboxState != BLACKBOX_STATE_ERASED) {
&& blackboxState != BLACKBOX_STATE_ERASED)
#endif
{
blackboxSetState(BLACKBOX_STATE_STOPPED);
// ensure we reset the test mode flag if we stop due to full memory card
if (startedLoggingInTestMode) {
startedLoggingInTestMode = false;
}
#ifdef USE_FLASHFS
}
#endif
} else { // Only log in test mode if there is room!
switch (blackboxConfig()->mode) {
case BLACKBOX_MODE_MOTOR_TEST:

2
src/main/common/maths.h
View File

@ -20,6 +20,8 @@
#pragma once
#include <stdint.h>
#ifndef sq
#define sq(x) ((x)*(x))
#endif

1
src/main/drivers/serial_usb_vcp.c
View File

@ -28,6 +28,7 @@
#include "build/build_config.h"
#include "common/utils.h"
#include "drivers/io.h"
#if defined(STM32F4)

2
src/main/drivers/serial_usb_vcp.h
View File

@ -23,6 +23,8 @@
#include "drivers/serial.h"
#if defined(STM32F7)
#include "common/maths.h"
#include "usbd_cdc.h"
extern USBD_HandleTypeDef USBD_Device;

34
src/main/fc/fc_core.c
View File

@ -40,12 +40,10 @@
#include "pg/rx.h"
#include "drivers/light_led.h"
#include "drivers/serial_usb_vcp.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/transponder_ir.h"
#include "drivers/usb_io.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
@ -67,7 +65,6 @@
#include "interface/cli.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "io/motors.h"
@ -905,33 +902,19 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
#endif
}
static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {
startTime = micros();
}
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY)) {
gyroReadTemperature();
}
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#ifdef USE_SDCARD
afatfs_poll();
#endif
#if defined(USE_VCP)
DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted());
DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected());
#endif
#ifdef USE_BLACKBOX
if (!cliMode && blackboxConfig()->device) {
blackboxUpdate(currentTimeUs);
@ -943,6 +926,16 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
#ifdef USE_TELEMETRY
void subTaskTelemetryPollSensors(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
// Read out gyro temperature if used for telemmetry
gyroReadTemperature();
}
#endif
static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
@ -1017,7 +1010,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
// 0 - gyroUpdate()
// 1 - subTaskPidController()
// 2 - subTaskMotorUpdate()
// 3 - subTaskMainSubprocesses()
// 3 - subTaskPidSubprocesses()
gyroUpdate(currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
@ -1025,7 +1018,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
subTaskRcCommand(currentTimeUs);
subTaskPidController(currentTimeUs);
subTaskMotorUpdate(currentTimeUs);
subTaskMainSubprocesses(currentTimeUs);
subTaskPidSubprocesses(currentTimeUs);
}
if (debugMode == DEBUG_CYCLETIME) {
@ -1054,4 +1047,3 @@ void resetTryingToArm()
{
tryingToArm = false;
}

2
src/main/fc/fc_core.h
View File

@ -56,3 +56,5 @@ bool isAirmodeActivated();
timeUs_t getLastDisarmTimeUs(void);
bool isTryingToArm();
void resetTryingToArm();
void subTaskTelemetryPollSensors(timeUs_t currentTimeUs);

58
src/main/fc/fc_tasks.c
View File

@ -38,15 +38,16 @@
#include "drivers/compass/compass.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/serial_usb_vcp.h"
#include "drivers/stack_check.h"
#include "drivers/transponder_ir.h"
#include "drivers/usb_io.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"
#include "fc/fc_dispatch.h"
#include "fc/fc_tasks.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@ -58,6 +59,7 @@
#include "interface/cli.h"
#include "interface/msp.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/dashboard.h"
#include "io/gps.h"
@ -93,20 +95,50 @@
#include "telemetry/telemetry.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#include "i2c_bst.h"
#endif
bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
#ifdef USE_USB_CDC_HID
//TODO: Make it platform independent in the future
#ifdef STM32F4
#include "vcpf4/usbd_cdc_vcp.h"
#include "usbd_hid_core.h"
#elif defined(STM32F7)
#include "usbd_cdc_interface.h"
#include "usbd_hid.h"
#endif
#endif
#include "fc_tasks.h"
static void taskMain(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
#ifdef USE_SDCARD
afatfs_poll();
#endif
}
#ifdef USE_OSD_SLAVE
static bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
UNUSED(currentTimeUs);
UNUSED(currentDeltaTimeUs);
return mspSerialWaiting();
}
#endif
static void taskHandleSerial(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
#if defined(USE_VCP)
DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted());
DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected());
#endif
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
@ -122,7 +154,7 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
}
void taskBatteryAlerts(timeUs_t currentTimeUs)
static void taskBatteryAlerts(timeUs_t currentTimeUs)
{
if (!ARMING_FLAG(ARMED)) {
// the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup.
@ -186,13 +218,15 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
static void taskTelemetry(timeUs_t currentTimeUs)
{
if (!cliMode && feature(FEATURE_TELEMETRY)) {
subTaskTelemetryPollSensors(currentTimeUs);
telemetryProcess(currentTimeUs);
}
}
#endif
#ifdef USE_CAMERA_CONTROL
void taskCameraControl(uint32_t currentTime)
static void taskCameraControl(uint32_t currentTime)
{
if (ARMING_FLAG(ARMED)) {
return;
@ -205,6 +239,9 @@ void taskCameraControl(uint32_t currentTime)
void fcTasksInit(void)
{
schedulerInit();
setTaskEnabled(TASK_MAIN, true);
setTaskEnabled(TASK_SERIAL, true);
rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
@ -320,11 +357,20 @@ void fcTasksInit(void)
cfTask_t cfTasks[TASK_COUNT] = {
[TASK_SYSTEM] = {
.taskName = "SYSTEM",
.taskFunc = taskSystem,
.subTaskName = "LOAD",
.taskFunc = taskSystemLoad,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
},
[TASK_MAIN] = {
.taskName = "SYSTEM",
.subTaskName = "UPDATE",
.taskFunc = taskMain,
.desiredPeriod = TASK_PERIOD_HZ(1000),
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
},
[TASK_SERIAL] = {
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,

2
src/main/scheduler/scheduler.c
View File

@ -123,7 +123,7 @@ FAST_CODE cfTask_t *queueNext(void)
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
}
void taskSystem(timeUs_t currentTimeUs)
void taskSystemLoad(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);

3
src/main/scheduler/scheduler.h
View File

@ -58,6 +58,7 @@ typedef struct {
typedef enum {
/* Actual tasks */
TASK_SYSTEM = 0,
TASK_MAIN,
TASK_GYROPID,
TASK_ACCEL,
TASK_ATTITUDE,
@ -179,7 +180,7 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId);
void schedulerInit(void);
void scheduler(void);
void taskSystem(timeUs_t currentTime);
void taskSystemLoad(timeUs_t currentTime);
#define LOAD_PERCENTAGE_ONE 100

8
src/main/target/SIRINFPV/target.h
View File

@ -22,6 +22,14 @@
#define TARGET_BOARD_IDENTIFIER "SIRF"
// Removed to make the firmware fit into flash (in descending order of priority):
#undef USE_EXTENDED_CMS_MENUS
#undef USE_RTC_TIME
#undef USE_RX_MSP
#undef USE_ESC_SENSOR_INFO
#define LED0_PIN PB2
#define USE_BEEPER
#define BEEPER_PIN PA1

6
src/main/vcp_hal/usbd_cdc_interface.c
View File

@ -46,6 +46,9 @@
*/
/* Includes ------------------------------------------------------------------*/
#include "drivers/serial_usb_vcp.h"
#include "drivers/time.h"
#include "stm32f7xx_hal.h"
#include "usbd_core.h"
#include "usbd_desc.h"
@ -53,9 +56,6 @@
#include "usbd_cdc_interface.h"
#include "stdbool.h"
#include "drivers/serial_usb_vcp.h"
#include "drivers/time.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define APP_RX_DATA_SIZE 2048

2
src/main/vcp_hal/usbd_cdc_interface.h
View File

@ -50,6 +50,8 @@
#define __USBD_CDC_IF_H
/* Includes ------------------------------------------------------------------*/
#include "common/maths.h"
#include "usbd_cdc.h"
#include "stm32f7xx_hal.h"
#include "usbd_core.h"

2
src/main/vcp_hal/usbd_conf.c
View File

@ -46,6 +46,8 @@
*/
/* Includes ------------------------------------------------------------------*/
#include "common/maths.h"
#include "stm32f7xx_hal.h"
#include "usbd_core.h"
#include "usbd_desc.h"

76
src/test/unit/scheduler_unittest.cc
View File

@ -69,7 +69,7 @@ extern "C" {
cfTask_t cfTasks[TASK_COUNT] = {
[TASK_SYSTEM] = {
.taskName = "SYSTEM",
.taskFunc = taskSystem,
.taskFunc = taskSystemLoad,
.desiredPeriod = TASK_PERIOD_HZ(10),
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
},
@ -122,8 +122,6 @@ extern "C" {
TEST(SchedulerUnittest, TestPriorites)
{
EXPECT_EQ(20, TASK_COUNT);
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority);
@ -238,64 +236,68 @@ TEST(SchedulerUnittest, TestQueueArray)
queueClear();
taskQueueArray[TASK_COUNT_UNITTEST + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear
unsigned enqueuedTasks = 0;
EXPECT_EQ(enqueuedTasks, taskQueueSize);
for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) {
if (cfTasks[taskId].taskFunc) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), true);
EXPECT_EQ(taskId + 1, taskQueueSize);
enqueuedTasks++;
EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
}
}
EXPECT_EQ(TASK_COUNT_UNITTEST - 1, taskQueueSize);
EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT_UNITTEST - 2]);
const cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT_UNITTEST - 2];
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]);
EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[enqueuedTasks - 1]);
const cfTask_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1];
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
setTaskEnabled(TASK_SYSTEM, false);
EXPECT_EQ(TASK_COUNT_UNITTEST - 2, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 3]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 2]); // NULL at end of queue
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]);
EXPECT_EQ(enqueuedTasks - 1, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 2]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]); // NULL at end of queue
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
taskQueueArray[TASK_COUNT_UNITTEST - 2] = 0;
taskQueueArray[enqueuedTasks - 1] = 0;
setTaskEnabled(TASK_SYSTEM, true);
EXPECT_EQ(TASK_COUNT_UNITTEST - 1, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 2]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]);
EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
cfTaskInfo_t taskInfo;
getTaskInfo(static_cast<cfTaskId_e>(TASK_COUNT_UNITTEST - 1), &taskInfo);
getTaskInfo(static_cast<cfTaskId_e>(enqueuedTasks + 1), &taskInfo);
EXPECT_EQ(false, taskInfo.isEnabled);
setTaskEnabled(static_cast<cfTaskId_e>(TASK_COUNT_UNITTEST - 1), true);
EXPECT_EQ(TASK_COUNT_UNITTEST, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]); // check no buffer overrun
setTaskEnabled(static_cast<cfTaskId_e>(enqueuedTasks), true);
EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); // check no buffer overrun
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
setTaskEnabled(TASK_SYSTEM, false);
EXPECT_EQ(TASK_COUNT_UNITTEST - 1, taskQueueSize);
//EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT_UNITTEST - 3]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]);
EXPECT_EQ(enqueuedTasks - 1, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
setTaskEnabled(TASK_ACCEL, false);
EXPECT_EQ(TASK_COUNT_UNITTEST - 2, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 2]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]);
EXPECT_EQ(enqueuedTasks - 2, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
setTaskEnabled(TASK_BATTERY_VOLTAGE, false);
EXPECT_EQ(TASK_COUNT_UNITTEST - 3, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 3]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 2]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST - 1]);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT_UNITTEST]);
EXPECT_EQ(enqueuedTasks - 2, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 2]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
}