Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Port iNav's rangefinder

Browse source
This commit is contained in:
jflyper 2017-12-14 23:44:22 +09:00
parent 8a9fd29dd3
commit 11c47c631b
53 changed files with 898 additions and 492 deletions
Download patch file Download diff file Expand all files Collapse all files

40
src/main/fc/fc_tasks.c
View file

@ -76,8 +76,8 @@
#include "sensors/compass.h"
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/rangefinder.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "scheduler/scheduler.h"
@ -87,10 +87,6 @@
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
#define TASK_PERIOD_US(us) (us)
bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
UNUSED(currentTimeUs);
@ -151,9 +147,9 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
}
#endif
#ifdef USE_SONAR
if (sensors(SENSOR_SONAR)) {
updateSonarAltHoldState();
#ifdef USE_RANGEFINDER
if (sensors(SENSOR_RANGEFINDER)) {
updateRangefinderAltHoldState();
}
#endif
#endif // USE_ALT_HOLD
@ -183,20 +179,20 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_BARO) || defined(USE_SONAR)
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
static void taskCalculateAltitude(timeUs_t currentTimeUs)
{
if (false
#if defined(USE_BARO)
|| (sensors(SENSOR_BARO) && isBaroReady())
#endif
#if defined(USE_SONAR)
|| sensors(SENSOR_SONAR)
#if defined(USE_RANGEFINDER)
|| sensors(SENSOR_RANGEFINDER)
#endif
) {
calculateEstimatedAltitude(currentTimeUs);
}}
#endif // USE_BARO || USE_SONAR
#endif // USE_BARO || USE_RANGEFINDER
#ifdef USE_TELEMETRY
static void taskTelemetry(timeUs_t currentTimeUs)
@ -292,11 +288,11 @@ void fcTasksInit(void)
#ifdef USE_BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef USE_SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#ifdef USE_RANGEFINDER
setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
#endif
#if defined(USE_BARO) || defined(USE_SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER));
#endif
#ifdef USE_DASHBOARD
setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
@ -492,16 +488,16 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
#ifdef USE_SONAR
[TASK_SONAR] = {
.taskName = "SONAR",
.taskFunc = sonarUpdate,
.desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfere with each other
#ifdef USE_RANGEFINDER
[TASK_RANGEFINDER] = {
.taskName = "RANGEFINDER",
.taskFunc = rangefinderUpdate,
.desiredPeriod = TASK_PERIOD_MS(70), // XXX HCSR04 sonar specific value.
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
#if defined(USE_BARO) || defined(USE_SONAR)
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
[TASK_ALTITUDE] = {
.taskName = "ALTITUDE",
.taskFunc = taskCalculateAltitude,