mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
added pidalt stuff into cli
added a modifier for set (type "set *") to see min/max values for each parameter. this is for gui stuff bumped config VERSION fixed 0xffff - now stuff in drv_pwm, after it was mentioned to me for the 3rd time :p replaced some uint8_t -> uint32_t for loop counters - saving 4 to 8 bytes of flash each time. thanks goes to thee 35+ years of C experience kicad guy. turned bitfiends into a regular struct - instant 100byte flash size reduction git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@174 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
c998f5ca67
commit
829331c020
12 changed files with 2747 additions and 2756 deletions
5396
obj/baseflight.hex
5396
obj/baseflight.hex
File diff suppressed because it is too large
Load diff
37
src/cli.c
37
src/cli.c
|
@ -23,7 +23,7 @@ extern const char rcChannelLetters[];
|
||||||
|
|
||||||
// buffer
|
// buffer
|
||||||
static char cliBuffer[32];
|
static char cliBuffer[32];
|
||||||
static uint8_t bufferIndex = 0;
|
static uint32_t bufferIndex = 0;
|
||||||
|
|
||||||
// sync this with MultiType enum from mw.h
|
// sync this with MultiType enum from mw.h
|
||||||
const char *mixerNames[] = {
|
const char *mixerNames[] = {
|
||||||
|
@ -151,6 +151,9 @@ const clivalue_t valueTable[] = {
|
||||||
{ "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200 },
|
{ "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200 },
|
||||||
{ "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200 },
|
{ "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200 },
|
||||||
{ "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200 },
|
{ "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200 },
|
||||||
|
{ "p_alt", VAR_UINT8, &cfg.P8[PIDALT], 0, 200 },
|
||||||
|
{ "i_alt", VAR_UINT8, &cfg.I8[PIDALT], 0, 200 },
|
||||||
|
{ "d_alt", VAR_UINT8, &cfg.D8[PIDALT], 0, 200 },
|
||||||
{ "p_level", VAR_UINT8, &cfg.P8[PIDLEVEL], 0, 200 },
|
{ "p_level", VAR_UINT8, &cfg.P8[PIDLEVEL], 0, 200 },
|
||||||
{ "i_level", VAR_UINT8, &cfg.I8[PIDLEVEL], 0, 200 },
|
{ "i_level", VAR_UINT8, &cfg.I8[PIDLEVEL], 0, 200 },
|
||||||
{ "d_level", VAR_UINT8, &cfg.D8[PIDLEVEL], 0, 200 },
|
{ "d_level", VAR_UINT8, &cfg.D8[PIDLEVEL], 0, 200 },
|
||||||
|
@ -159,7 +162,7 @@ const clivalue_t valueTable[] = {
|
||||||
#define VALUE_COUNT (sizeof(valueTable) / sizeof(valueTable[0]))
|
#define VALUE_COUNT (sizeof(valueTable) / sizeof(valueTable[0]))
|
||||||
|
|
||||||
static void cliSetVar(const clivalue_t *var, const int32_t value);
|
static void cliSetVar(const clivalue_t *var, const int32_t value);
|
||||||
static void cliPrintVar(const clivalue_t *var);
|
static void cliPrintVar(const clivalue_t *var, uint32_t full);
|
||||||
|
|
||||||
#ifndef HAVE_ITOA_FUNCTION
|
#ifndef HAVE_ITOA_FUNCTION
|
||||||
|
|
||||||
|
@ -231,8 +234,8 @@ static void cliExit(char *cmdline)
|
||||||
|
|
||||||
static void cliFeature(char *cmdline)
|
static void cliFeature(char *cmdline)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
uint8_t len;
|
uint32_t len;
|
||||||
uint32_t mask;
|
uint32_t mask;
|
||||||
|
|
||||||
len = strlen(cmdline);
|
len = strlen(cmdline);
|
||||||
|
@ -290,7 +293,7 @@ static void cliFeature(char *cmdline)
|
||||||
|
|
||||||
static void cliHelp(char *cmdline)
|
static void cliHelp(char *cmdline)
|
||||||
{
|
{
|
||||||
uint8_t i = 0;
|
uint32_t i = 0;
|
||||||
|
|
||||||
uartPrint("Available commands:\r\n");
|
uartPrint("Available commands:\r\n");
|
||||||
|
|
||||||
|
@ -305,8 +308,8 @@ static void cliHelp(char *cmdline)
|
||||||
|
|
||||||
static void cliMap(char *cmdline)
|
static void cliMap(char *cmdline)
|
||||||
{
|
{
|
||||||
uint8_t len;
|
uint32_t len;
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
char out[9];
|
char out[9];
|
||||||
|
|
||||||
len = strlen(cmdline);
|
len = strlen(cmdline);
|
||||||
|
@ -379,7 +382,7 @@ static void cliSave(char *cmdline)
|
||||||
systemReset(false);
|
systemReset(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void cliPrintVar(const clivalue_t *var)
|
static void cliPrintVar(const clivalue_t *var, uint32_t full)
|
||||||
{
|
{
|
||||||
int32_t value = 0;
|
int32_t value = 0;
|
||||||
char buf[16];
|
char buf[16];
|
||||||
|
@ -407,6 +410,14 @@ static void cliPrintVar(const clivalue_t *var)
|
||||||
}
|
}
|
||||||
itoa(value, buf, 10);
|
itoa(value, buf, 10);
|
||||||
uartPrint(buf);
|
uartPrint(buf);
|
||||||
|
if (full) {
|
||||||
|
uartPrint(" ");
|
||||||
|
itoa(var->min, buf, 10);
|
||||||
|
uartPrint(buf);
|
||||||
|
uartPrint(" ");
|
||||||
|
itoa(var->max, buf, 10);
|
||||||
|
uartPrint(buf);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void cliSetVar(const clivalue_t *var, const int32_t value)
|
static void cliSetVar(const clivalue_t *var, const int32_t value)
|
||||||
|
@ -430,21 +441,21 @@ static void cliSetVar(const clivalue_t *var, const int32_t value)
|
||||||
|
|
||||||
static void cliSet(char *cmdline)
|
static void cliSet(char *cmdline)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
uint8_t len;
|
uint32_t len;
|
||||||
const clivalue_t *val;
|
const clivalue_t *val;
|
||||||
char *eqptr = NULL;
|
char *eqptr = NULL;
|
||||||
int32_t value = 0;
|
int32_t value = 0;
|
||||||
|
|
||||||
len = strlen(cmdline);
|
len = strlen(cmdline);
|
||||||
|
|
||||||
if (len == 0) {
|
if (len == 0 || (len == 1 && cmdline[0] == '*')) {
|
||||||
uartPrint("Current settings: \r\n");
|
uartPrint("Current settings: \r\n");
|
||||||
for (i = 0; i < VALUE_COUNT; i++) {
|
for (i = 0; i < VALUE_COUNT; i++) {
|
||||||
val = &valueTable[i];
|
val = &valueTable[i];
|
||||||
uartPrint((char *)valueTable[i].name);
|
uartPrint((char *)valueTable[i].name);
|
||||||
uartPrint(" = ");
|
uartPrint(" = ");
|
||||||
cliPrintVar(val);
|
cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
|
||||||
uartPrint("\r\n");
|
uartPrint("\r\n");
|
||||||
while (!uartTransmitEmpty());
|
while (!uartTransmitEmpty());
|
||||||
}
|
}
|
||||||
|
@ -461,7 +472,7 @@ static void cliSet(char *cmdline)
|
||||||
cliSetVar(val, value);
|
cliSetVar(val, value);
|
||||||
uartPrint((char *)valueTable[i].name);
|
uartPrint((char *)valueTable[i].name);
|
||||||
uartPrint(" set to ");
|
uartPrint(" set to ");
|
||||||
cliPrintVar(val);
|
cliPrintVar(val, 0);
|
||||||
} else {
|
} else {
|
||||||
uartPrint("ERR: Value assignment out of range\r\n");
|
uartPrint("ERR: Value assignment out of range\r\n");
|
||||||
}
|
}
|
||||||
|
|
|
@ -13,7 +13,7 @@ config_t cfg;
|
||||||
const char rcChannelLetters[] = "AERT1234";
|
const char rcChannelLetters[] = "AERT1234";
|
||||||
|
|
||||||
static uint32_t enabledSensors = 0;
|
static uint32_t enabledSensors = 0;
|
||||||
uint8_t checkNewConf = 19;
|
uint8_t checkNewConf = 22;
|
||||||
|
|
||||||
void parseRcChannels(const char *input)
|
void parseRcChannels(const char *input)
|
||||||
{
|
{
|
||||||
|
|
|
@ -79,11 +79,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
|
||||||
|
|
||||||
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
|
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
|
||||||
|
|
||||||
if (now > last) {
|
diff = now - last;
|
||||||
diff = (now - last);
|
|
||||||
} else {
|
|
||||||
diff = ((0xFFFF - last) + now);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe."
|
if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe."
|
||||||
chan = 0;
|
chan = 0;
|
||||||
|
@ -98,7 +94,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
|
||||||
|
|
||||||
static void pwmIRQHandler(TIM_TypeDef *tim)
|
static void pwmIRQHandler(TIM_TypeDef *tim)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
uint16_t val = 0;
|
uint16_t val = 0;
|
||||||
|
|
||||||
for (i = 0; i < 8; i++) {
|
for (i = 0; i < 8; i++) {
|
||||||
|
@ -162,7 +158,7 @@ static void pwmInitializeInput(bool usePPM)
|
||||||
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
||||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
||||||
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
|
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
|
|
||||||
// Input pins
|
// Input pins
|
||||||
if (usePPM) {
|
if (usePPM) {
|
||||||
|
|
|
@ -45,7 +45,7 @@ uint32_t millis(void)
|
||||||
void systemInit(void)
|
void systemInit(void)
|
||||||
{
|
{
|
||||||
GPIO_InitTypeDef GPIO_InitStructure;
|
GPIO_InitTypeDef GPIO_InitStructure;
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
|
|
||||||
gpio_config_t gpio_cfg[] = {
|
gpio_config_t gpio_cfg[] = {
|
||||||
{ LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP }, // PB3 (LED)
|
{ LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP }, // PB3 (LED)
|
||||||
|
|
|
@ -621,7 +621,7 @@ static uint32_t GPS_coord_to_degrees(char *s)
|
||||||
// helper functions
|
// helper functions
|
||||||
static uint32_t grab_fields(char *src, uint8_t mult)
|
static uint32_t grab_fields(char *src, uint8_t mult)
|
||||||
{ // convert string to uint32
|
{ // convert string to uint32
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
uint32_t tmp = 0;
|
uint32_t tmp = 0;
|
||||||
for (i = 0; src[i] != 0; i++) {
|
for (i = 0; src[i] != 0; i++) {
|
||||||
if (src[i] == '.') {
|
if (src[i] == '.') {
|
||||||
|
|
|
@ -34,7 +34,7 @@ void imuInit(void)
|
||||||
|
|
||||||
void computeIMU(void)
|
void computeIMU(void)
|
||||||
{
|
{
|
||||||
uint8_t axis;
|
uint32_t axis;
|
||||||
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
|
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
|
||||||
int16_t gyroADCp[3];
|
int16_t gyroADCp[3];
|
||||||
int16_t gyroADCinter[3];
|
int16_t gyroADCinter[3];
|
||||||
|
@ -180,7 +180,7 @@ static int16_t _atan2f(float y, float x)
|
||||||
|
|
||||||
static void getEstimatedAttitude(void)
|
static void getEstimatedAttitude(void)
|
||||||
{
|
{
|
||||||
uint8_t axis;
|
uint32_t axis;
|
||||||
int32_t accMag = 0;
|
int32_t accMag = 0;
|
||||||
static t_fp_vector EstG;
|
static t_fp_vector EstG;
|
||||||
static t_fp_vector EstM;
|
static t_fp_vector EstM;
|
||||||
|
@ -265,10 +265,10 @@ static void getEstimatedAttitude(void)
|
||||||
|
|
||||||
void getEstimatedAltitude(void)
|
void getEstimatedAltitude(void)
|
||||||
{
|
{
|
||||||
uint8_t index;
|
uint32_t index;
|
||||||
static uint32_t deadLine = INIT_DELAY;
|
static uint32_t deadLine = INIT_DELAY;
|
||||||
static int16_t BaroHistTab[BARO_TAB_SIZE];
|
static int16_t BaroHistTab[BARO_TAB_SIZE];
|
||||||
static int8_t BaroHistIdx;
|
static uint32_t BaroHistIdx;
|
||||||
static int32_t BaroHigh = 0;
|
static int32_t BaroHigh = 0;
|
||||||
static int32_t BaroLow = 0;
|
static int32_t BaroLow = 0;
|
||||||
int32_t temp32;
|
int32_t temp32;
|
||||||
|
|
|
@ -97,7 +97,7 @@ void writeAllMotors(int16_t mc)
|
||||||
void mixTable(void)
|
void mixTable(void)
|
||||||
{
|
{
|
||||||
int16_t maxMotor;
|
int16_t maxMotor;
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
static uint8_t camCycle = 0;
|
static uint8_t camCycle = 0;
|
||||||
static uint8_t camState = 0;
|
static uint8_t camState = 0;
|
||||||
static uint32_t camTime = 0;
|
static uint32_t camTime = 0;
|
||||||
|
|
1
src/mw.c
1
src/mw.c
|
@ -43,7 +43,6 @@ uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS positi
|
||||||
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
|
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
|
||||||
uint16_t GPS_ground_course = 0; // degrees*10
|
uint16_t GPS_ground_course = 0; // degrees*10
|
||||||
uint8_t GPS_Present = 0; // Checksum from Gps serial
|
uint8_t GPS_Present = 0; // Checksum from Gps serial
|
||||||
uint8_t GPS_Enable = 0;
|
|
||||||
int16_t nav[2];
|
int16_t nav[2];
|
||||||
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
|
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
|
||||||
|
|
||||||
|
|
31
src/mw.h
31
src/mw.h
|
@ -198,21 +198,21 @@ typedef struct config_t {
|
||||||
} config_t;
|
} config_t;
|
||||||
|
|
||||||
typedef struct flags_t {
|
typedef struct flags_t {
|
||||||
uint8_t OK_TO_ARM :1;
|
uint8_t OK_TO_ARM;
|
||||||
uint8_t ARMED :1;
|
uint8_t ARMED;
|
||||||
uint8_t I2C_INIT_DONE :1; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup())
|
uint8_t I2C_INIT_DONE; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup())
|
||||||
uint8_t ACC_CALIBRATED :1;
|
uint8_t ACC_CALIBRATED;
|
||||||
uint8_t ACC_MODE :1;
|
uint8_t ACC_MODE;
|
||||||
uint8_t MAG_MODE :1;
|
uint8_t MAG_MODE;
|
||||||
uint8_t BARO_MODE :1;
|
uint8_t BARO_MODE;
|
||||||
uint8_t GPS_HOME_MODE :1;
|
uint8_t GPS_HOME_MODE;
|
||||||
uint8_t GPS_HOLD_MODE :1;
|
uint8_t GPS_HOLD_MODE;
|
||||||
uint8_t HEADFREE_MODE :1;
|
uint8_t HEADFREE_MODE;
|
||||||
uint8_t PASSTHRU_MODE :1;
|
uint8_t PASSTHRU_MODE;
|
||||||
uint8_t GPS_FIX :1;
|
uint8_t GPS_FIX;
|
||||||
uint8_t GPS_FIX_HOME :1;
|
uint8_t GPS_FIX_HOME;
|
||||||
uint8_t SMALL_ANGLES_25 :1;
|
uint8_t SMALL_ANGLES_25;
|
||||||
uint8_t CALIBRATE_MAG :1;
|
uint8_t CALIBRATE_MAG;
|
||||||
} flags_t;
|
} flags_t;
|
||||||
|
|
||||||
extern int16_t gyroZero[3];
|
extern int16_t gyroZero[3];
|
||||||
|
@ -264,7 +264,6 @@ extern uint8_t GPS_update; // it's a binary to
|
||||||
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
||||||
extern uint16_t GPS_ground_course; // degrees*10
|
extern uint16_t GPS_ground_course; // degrees*10
|
||||||
extern uint8_t GPS_Present; // Checksum from Gps serial
|
extern uint8_t GPS_Present; // Checksum from Gps serial
|
||||||
extern uint8_t GPS_Enable;
|
|
||||||
extern int16_t nav[2];
|
extern int16_t nav[2];
|
||||||
extern int8_t nav_mode; // Navigation mode
|
extern int8_t nav_mode; // Navigation mode
|
||||||
|
|
||||||
|
|
|
@ -115,7 +115,7 @@ uint16_t batteryAdcToVoltage(uint16_t src)
|
||||||
|
|
||||||
void batteryInit(void)
|
void batteryInit(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
uint32_t voltage = 0;
|
uint32_t voltage = 0;
|
||||||
|
|
||||||
// average up some voltage readings
|
// average up some voltage readings
|
||||||
|
@ -138,7 +138,7 @@ void batteryInit(void)
|
||||||
static void ACC_Common(void)
|
static void ACC_Common(void)
|
||||||
{
|
{
|
||||||
static int32_t a[3];
|
static int32_t a[3];
|
||||||
uint8_t axis;
|
uint32_t axis;
|
||||||
|
|
||||||
if (calibratingA > 0) {
|
if (calibratingA > 0) {
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
|
@ -272,7 +272,7 @@ static void GYRO_Common(void)
|
||||||
{
|
{
|
||||||
static int16_t previousGyroADC[3] = { 0, 0, 0 };
|
static int16_t previousGyroADC[3] = { 0, 0, 0 };
|
||||||
static int32_t g[3];
|
static int32_t g[3];
|
||||||
uint8_t axis;
|
uint32_t axis;
|
||||||
|
|
||||||
if (calibratingG > 0) {
|
if (calibratingG > 0) {
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
|
@ -328,7 +328,7 @@ void Mag_init(void)
|
||||||
{
|
{
|
||||||
uint8_t calibration_gain = 0x60; // HMC5883
|
uint8_t calibration_gain = 0x60; // HMC5883
|
||||||
#if 1
|
#if 1
|
||||||
uint8_t numAttempts = 0, good_count = 0;
|
uint32_t numAttempts = 0, good_count = 0;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
uint16_t expected_x = 766; // default values for HMC5883
|
uint16_t expected_x = 766; // default values for HMC5883
|
||||||
uint16_t expected_yz = 713;
|
uint16_t expected_yz = 713;
|
||||||
|
@ -405,7 +405,7 @@ void Mag_getADC(void)
|
||||||
static uint32_t t, tCal = 0;
|
static uint32_t t, tCal = 0;
|
||||||
static int16_t magZeroTempMin[3];
|
static int16_t magZeroTempMin[3];
|
||||||
static int16_t magZeroTempMax[3];
|
static int16_t magZeroTempMax[3];
|
||||||
uint8_t axis;
|
uint32_t axis;
|
||||||
|
|
||||||
if (currentTime < t)
|
if (currentTime < t)
|
||||||
return; //each read is spaced by 100ms
|
return; //each read is spaced by 100ms
|
||||||
|
|
|
@ -169,7 +169,7 @@ void serialInit(uint32_t baudrate)
|
||||||
|
|
||||||
static void evaluateCommand(void)
|
static void evaluateCommand(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint32_t i;
|
||||||
|
|
||||||
switch (cmdMSP) {
|
switch (cmdMSP) {
|
||||||
case MSP_SET_RAW_RC:
|
case MSP_SET_RAW_RC:
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue