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

Merge remote-tracking branch 'betaflight/master' into bfdev-add-gyro-to-MSP_STATUS_EX

This commit is contained in:
jflyper 2017-06-30 22:20:57 +09:00
parent bdeb004bd0 740b4a91bd
commit b3a481e95a
10 changed files with 76 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

@ -1,4 +1,6 @@
![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067)
![BetaFlight Notice, version 3.2 will be the last version of Betaflight to support STM32F1 based flight controllers, this includes NAZE, CC3D (original) and CJMCU like flight controllers](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf3_2_notice.png)
![BetaFlight](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf_logo.png)
Betaflight is flight controller software (firmware) used to fly multi-rotor craft and fixed wing craft.

9
src/main/drivers/system.c
View File

@ -164,9 +164,8 @@ void delay(uint32_t ms)
#define SHORT_FLASH_DURATION 50
#define CODE_FLASH_DURATION 250
void failureMode(failureMode_e mode)
void failureLedCode(failureMode_e mode, int codeRepeatsRemaining)
{
int codeRepeatsRemaining = 10;
int codeFlashesRemaining;
int shortFlashesRemaining;
@ -201,6 +200,12 @@ void failureMode(failureMode_e mode)
delay(1000);
}
}
void failureMode(failureMode_e mode)
{
failureLedCode(mode, 10);
#ifdef DEBUG
systemReset();
#else

1
src/main/drivers/system.h
View File

@ -33,6 +33,7 @@ typedef enum {
} failureMode_e;
// failure
void failureLedCode(failureMode_e mode, int repeatCount);
void failureMode(failureMode_e mode);
// bootloader/IAP

6
src/main/fc/cli.c
View File

@ -2711,7 +2711,11 @@ static void cliStatus(char *cmdline)
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
#ifdef MINIMAL_CLI
cliPrintLinef("0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI);
#else
cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI);
#endif
}
#ifndef SKIP_TASK_STATISTICS

5
src/main/fc/fc_init.c
View File

@ -478,8 +478,9 @@ void init(void)
initBoardAlignment(boardAlignment());
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
// if gyro was not detected due to whatever reason, notify and don't arm.
failureLedCode(FAILURE_MISSING_ACC, 2);
setArmingDisabled(ARMING_DISABLED_NO_GYRO);
}
systemState |= SYSTEM_STATE_SENSORS_READY;

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

@ -259,8 +259,11 @@ void osdSlaveTasksInit(void)
void fcTasksInit(void)
{
schedulerInit();
if (sensors(SENSOR_GYRO)) {
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
}
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);

5
src/main/fc/runtime_config.c
View File

@ -46,6 +46,11 @@ bool isArmingDisabled()
return armingDisableFlags;
}
armingDisableFlags_e getArmingDisableFlags(void)
{
return armingDisableFlags;
}
/**
* Enables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.

26
src/main/fc/runtime_config.h
View File

@ -29,22 +29,28 @@ extern uint8_t armingFlags;
#define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask))
#define ARMING_FLAG(mask) (armingFlags & (mask))
/*
* Arming disable flags are listed in the order of criticalness.
* (Beeper code can notify the most critical reason.)
*/
typedef enum {
ARMING_DISABLED_FAILSAFE = (1 << 0),
ARMING_DISABLED_BOXFAILSAFE = (1 << 1),
ARMING_DISABLED_THROTTLE = (1 << 2),
ARMING_DISABLED_ANGLE = (1 << 3),
ARMING_DISABLED_LOAD = (1 << 4),
ARMING_DISABLED_CALIBRATING = (1 << 5),
ARMING_DISABLED_CLI = (1 << 6),
ARMING_DISABLED_CMS_MENU = (1 << 7),
ARMING_DISABLED_OSD_MENU = (1 << 8),
ARMING_DISABLED_BST = (1 << 9),
ARMING_DISABLED_NO_GYRO = (1 << 0),
ARMING_DISABLED_FAILSAFE = (1 << 1),
ARMING_DISABLED_BOXFAILSAFE = (1 << 2),
ARMING_DISABLED_THROTTLE = (1 << 3),
ARMING_DISABLED_ANGLE = (1 << 4),
ARMING_DISABLED_LOAD = (1 << 5),
ARMING_DISABLED_CALIBRATING = (1 << 6),
ARMING_DISABLED_CLI = (1 << 7),
ARMING_DISABLED_CMS_MENU = (1 << 8),
ARMING_DISABLED_OSD_MENU = (1 << 9),
ARMING_DISABLED_BST = (1 << 10),
} armingDisableFlags_e;
void setArmingDisabled(armingDisableFlags_e flag);
void unsetArmingDisabled(armingDisableFlags_e flag);
bool isArmingDisabled(void);
armingDisableFlags_e getArmingDisableFlags(void);
typedef enum {
ANGLE_MODE = (1 << 0),

38
src/main/flight/navigation.c
View File

@ -107,19 +107,22 @@ void navigationInit(void)
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
static bool check_missed_wp(void);
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
#ifdef USE_NAV
static bool check_missed_wp(void);
static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
static int32_t wrap_36000(int32_t angle);
#endif
static int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
int16_t last_velocity;
@ -134,7 +137,6 @@ typedef struct {
static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
typedef struct {
float integrator; // integrator value
@ -146,6 +148,9 @@ typedef struct {
static PID posholdPID[2];
static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2];
static int32_t get_P(int32_t error, PID_PARAM *pid)
@ -176,6 +181,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// add in derivative component
return pid_param->kD * pid->derivative;
}
#endif
static void reset_PID(PID *pid)
{
@ -197,11 +203,15 @@ static void reset_PID(PID *pid)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
static int32_t error[2];
#ifdef USE_NAV
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
#endif
// Currently used WP
static int32_t GPS_WP[2];
@ -217,8 +227,6 @@ static int32_t target_bearing;
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing;
@ -265,10 +273,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void)
{
int axis;
static uint32_t nav_loopTimer;
uint16_t speed;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
@ -283,7 +288,7 @@ void onGpsNewData(void)
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) {
for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
@ -321,6 +326,7 @@ void onGpsNewData(void)
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
#ifdef USE_NAV
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating
@ -328,6 +334,7 @@ void onGpsNewData(void)
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
uint16_t speed;
switch (nav_mode) {
case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@ -360,6 +367,7 @@ void onGpsNewData(void)
break;
}
} //end of gps calcs
#endif
}
void GPS_reset_home_position(void)
@ -385,7 +393,9 @@ void GPS_reset_nav(void)
nav[i] = 0;
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
#ifdef USE_NAV
reset_PID(&navPID[i]);
#endif
}
}
@ -401,10 +411,12 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#ifdef USE_NAV
navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif
}
// OK here is the onboard GPS code
@ -442,6 +454,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
waypoint_speed_gov = navigationConfig()->nav_speed_min;
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
@ -452,6 +465,7 @@ static bool check_missed_wp(void)
temp = wrap_18000(temp);
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
}
#endif
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
@ -522,6 +536,7 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
error[LAT] = *target_lat - *gps_lat; // Y Error
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
@ -627,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
}
return max_speed;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
// Utilities
@ -640,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
return error;
}
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
@ -648,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000;
return angle;
}
#endif
void updateGpsStateForHomeAndHoldMode(void)
{

7
src/main/target/common_fc_pre.h
View File

@ -115,6 +115,7 @@
#define TELEMETRY_SRXL
#define USE_DASHBOARD
#define USE_MSP_DISPLAYPORT
#define USE_RCSPLIT
#define USE_RX_MSP
#define USE_SERIALRX_JETIEXBUS
#define USE_SENSOR_NAMES
@ -131,10 +132,8 @@
#endif
#if (FLASH_SIZE > 256)
// Temporarily moved this here because of overflowing flash size on F3
// Temporarily moved GPS here because of overflowing flash size on F3
#define GPS
#define USE_NAV
#define USE_UNCOMMON_MIXERS
#endif
#define USE_RCSPLIT