Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

begin initial merge of 2.2 features

Browse source 
mw2.2-merged stuff:
* implemented profiles 0-2 (called 'setting' in mwiigui)
* merged in MSP changes including profile switch
* cleaned up rc / aux switch stuff in mw.c based on 2.2 rewrite
* main loop switch for baro/sonar shit adjusted
todo: basically the rest of 2.2 changes (i think some minor imu/gps/baro updates)
baseflight-specific stuff:
* made boxitems transmission dynamic, based on enabled features. no more GPS / camstab trash if it's not enabled
* cleaned up gyro drivers to return scale factor to imu code
* set gyro_lpf now controls every supported gyro - not all take same values, see driver files for allowed ranges, in case of invalid lpf, defaults to something reasonable (around 30-40hz)

maybe couple other things I forgot. this is all 100% experimental, untested, and not even flown. thats why there's  no hex.
merge is still ongoing.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@294 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-03-14 14:03:30 +00:00
parent bba9bc291f
commit 491b3627f6
20 changed files with 1009 additions and 559 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/board.h
View file

@ -56,6 +56,7 @@ typedef enum {
FEATURE_SONAR = 1 << 10,
FEATURE_TELEMETRY = 1 << 11,
FEATURE_POWERMETER = 1 << 12,
FEATURE_VARIO = 1 << 13,
} AvailableFeatures;
typedef enum {
@ -72,10 +73,11 @@ typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver
typedef struct sensor_t
{
sensorInitFuncPtr init;
sensorReadFuncPtr read;
sensorReadFuncPtr align;
sensorReadFuncPtr temperature;
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr align; // sensor align
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor (currently used for gyro only, todo for accel)
} sensor_t;
typedef struct baro_t

150
src/cli.c
View file

@ -12,6 +12,7 @@ static void cliFeature(char *cmdline);
static void cliHelp(char *cmdline);
static void cliMap(char *cmdline);
static void cliMixer(char *cmdline);
static void cliProfile(char *cmdline);
static void cliSave(char *cmdline);
static void cliSet(char *cmdline);
static void cliStatus(char *cmdline);
@ -43,7 +44,7 @@ const char * const mixerNames[] = {
const char * const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY",
"FAILSAFE", "SONAR", "TELEMETRY", "VARIO",
NULL
};
@ -74,6 +75,7 @@ const clicmd_t cmdTable[] = {
{ "help", "", cliHelp },
{ "map", "mapping of rc channel order", cliMap },
{ "mixer", "mixer name or list", cliMixer },
{ "profile", "index (0 to 2)", cliProfile },
{ "save", "save and reboot", cliSave },
{ "set", "name=value or blank or * for list", cliSet },
{ "status", "show system status", cliStatus },
@ -99,34 +101,50 @@ typedef struct {
} clivalue_t;
const clivalue_t valueTable[] = {
{ "looptime", VAR_UINT16, &mcfg.looptime, 0, 9000 },
{ "midrc", VAR_UINT16, &mcfg.midrc, 1200, 1700 },
{ "minthrottle", VAR_UINT16, &mcfg.minthrottle, 0, 2000 },
{ "maxthrottle", VAR_UINT16, &mcfg.maxthrottle, 0, 2000 },
{ "mincommand", VAR_UINT16, &mcfg.mincommand, 0, 2000 },
{ "mincheck", VAR_UINT16, &mcfg.mincheck, 0, 2000 },
{ "maxcheck", VAR_UINT16, &mcfg.maxcheck, 0, 2000 },
{ "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
{ "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 498 },
{ "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 },
{ "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
{ "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 },
{ "spektrum_hires", VAR_UINT8, &mcfg.spektrum_hires, 0, 1 },
{ "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
{ "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 },
{ "align_gyro_x", VAR_INT8, &mcfg.align[ALIGN_GYRO][0], -3, 3 },
{ "align_gyro_y", VAR_INT8, &mcfg.align[ALIGN_GYRO][1], -3, 3 },
{ "align_gyro_z", VAR_INT8, &mcfg.align[ALIGN_GYRO][2], -3, 3 },
{ "align_acc_x", VAR_INT8, &mcfg.align[ALIGN_ACCEL][0], -3, 3 },
{ "align_acc_y", VAR_INT8, &mcfg.align[ALIGN_ACCEL][1], -3, 3 },
{ "align_acc_z", VAR_INT8, &mcfg.align[ALIGN_ACCEL][2], -3, 3 },
{ "align_mag_x", VAR_INT8, &mcfg.align[ALIGN_MAG][0], -3, 3 },
{ "align_mag_y", VAR_INT8, &mcfg.align[ALIGN_MAG][1], -3, 3 },
{ "align_mag_z", VAR_INT8, &mcfg.align[ALIGN_MAG][2], -3, 3 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 3 },
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
{ "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
{ "midrc", VAR_UINT16, &cfg.midrc, 1200, 1700 },
{ "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },
{ "thr_expo", VAR_UINT8, &cfg.thrExpo8, 0, 250 },
{ "roll_pitch_rate", VAR_UINT8, &cfg.rollPitchRate, 0, 100 },
{ "yawrate", VAR_UINT8, &cfg.yawRate, 0, 100 },
{ "minthrottle", VAR_UINT16, &cfg.minthrottle, 0, 2000 },
{ "maxthrottle", VAR_UINT16, &cfg.maxthrottle, 0, 2000 },
{ "mincommand", VAR_UINT16, &cfg.mincommand, 0, 2000 },
{ "mincheck", VAR_UINT16, &cfg.mincheck, 0, 2000 },
{ "maxcheck", VAR_UINT16, &cfg.maxcheck, 0, 2000 },
{ "retarded_arm", VAR_UINT8, &cfg.retarded_arm, 0, 1 },
{ "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
{ "motor_pwm_rate", VAR_UINT16, &cfg.motor_pwm_rate, 50, 498 },
{ "servo_pwm_rate", VAR_UINT16, &cfg.servo_pwm_rate, 50, 498 },
{ "serial_baudrate", VAR_UINT32, &cfg.serial_baudrate, 1200, 115200 },
{ "gps_baudrate", VAR_UINT32, &cfg.gps_baudrate, 1200, 115200 },
{ "spektrum_hires", VAR_UINT8, &cfg.spektrum_hires, 0, 1 },
{ "vbatscale", VAR_UINT8, &cfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &cfg.vbatmincellvoltage, 10, 50 },
{ "power_adc_channel", VAR_UINT8, &cfg.power_adc_channel, 0, 9 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
@ -150,28 +168,14 @@ const clivalue_t valueTable[] = {
{ "gimbal_roll_min", VAR_UINT16, &cfg.gimbal_roll_min, 100, 3000 },
{ "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 },
{ "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 },
{ "align_gyro_x", VAR_INT8, &cfg.align[ALIGN_GYRO][0], -3, 3 },
{ "align_gyro_y", VAR_INT8, &cfg.align[ALIGN_GYRO][1], -3, 3 },
{ "align_gyro_z", VAR_INT8, &cfg.align[ALIGN_GYRO][2], -3, 3 },
{ "align_acc_x", VAR_INT8, &cfg.align[ALIGN_ACCEL][0], -3, 3 },
{ "align_acc_y", VAR_INT8, &cfg.align[ALIGN_ACCEL][1], -3, 3 },
{ "align_acc_z", VAR_INT8, &cfg.align[ALIGN_ACCEL][2], -3, 3 },
{ "align_mag_x", VAR_INT8, &cfg.align[ALIGN_MAG][0], -3, 3 },
{ "align_mag_y", VAR_INT8, &cfg.align[ALIGN_MAG][1], -3, 3 },
{ "align_mag_z", VAR_INT8, &cfg.align[ALIGN_MAG][2], -3, 3 },
{ "acc_hardware", VAR_UINT8, &cfg.acc_hardware, 0, 3 },
{ "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
{ "acc_lpf_for_velocity", VAR_UINT8, &cfg.acc_lpf_for_velocity, 1, 250 },
{ "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 },
{ "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 },
{ "gyro_lpf", VAR_UINT16, &cfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &cfg.gyro_cmpf_factor, 100, 1000 },
{ "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
{ "baro_noise_lpf", VAR_FLOAT, &cfg.baro_noise_lpf, 0, 1 },
{ "baro_cf", VAR_FLOAT, &cfg.baro_cf, 0, 1 },
{ "moron_threshold", VAR_UINT8, &cfg.moron_threshold, 0, 128 },
{ "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 },
{ "gps_type", VAR_UINT8, &cfg.gps_type, 0, 3 },
{ "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8, &cfg.D8[PIDPOS], 0, 200 },
@ -186,7 +190,6 @@ const clivalue_t valueTable[] = {
{ "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &cfg.nav_slew_rate, 0, 100 },
{ "looptime", VAR_UINT16, &cfg.looptime, 0, 9000 },
{ "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 },
@ -435,26 +438,30 @@ static void cliCMix(char *cmdline)
if (len == 0) {
uartPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
for (i = 0; i < MAX_MOTORS; i++) {
if (cfg.customMixer[i].throttle == 0.0f)
if (mcfg.customMixer[i].throttle == 0.0f)
break;
mixsum[i] = 0.0f;
num_motors++;
printf("#%d:\t", i + 1);
printf("%s\t", ftoa(cfg.customMixer[i].throttle, buf));
printf("%s\t", ftoa(cfg.customMixer[i].roll, buf));
printf("%s\t", ftoa(cfg.customMixer[i].pitch, buf));
printf("%s\r\n", ftoa(cfg.customMixer[i].yaw, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].throttle, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].roll, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].pitch, buf));
printf("%s\r\n", ftoa(mcfg.customMixer[i].yaw, buf));
}
mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
for (i = 0; i < num_motors; i++) {
mixsum[0] += cfg.customMixer[i].roll;
mixsum[1] += cfg.customMixer[i].pitch;
mixsum[2] += cfg.customMixer[i].yaw;
mixsum[0] += mcfg.customMixer[i].roll;
mixsum[1] += mcfg.customMixer[i].pitch;
mixsum[2] += mcfg.customMixer[i].yaw;
}
uartPrint("Sanity check:\t");
for (i = 0; i < 3; i++)
uartPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
uartPrint("\r\n");
return;
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
for (i = 0; i < MAX_MOTORS; i++)
mcfg.customMixer[i].throttle = 0.0f;
} else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = strchr(cmdline, ' ');
if (ptr) {
@ -478,22 +485,22 @@ static void cliCMix(char *cmdline)
if (--i < MAX_MOTORS) {
ptr = strchr(ptr, ' ');
if (ptr) {
cfg.customMixer[i].throttle = _atof(++ptr);
mcfg.customMixer[i].throttle = _atof(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
cfg.customMixer[i].roll = _atof(++ptr);
mcfg.customMixer[i].roll = _atof(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
cfg.customMixer[i].pitch = _atof(++ptr);
mcfg.customMixer[i].pitch = _atof(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
cfg.customMixer[i].yaw = _atof(++ptr);
mcfg.customMixer[i].yaw = _atof(++ptr);
check++;
}
if (check != 4) {
@ -519,7 +526,7 @@ static void cliDefaults(char *cmdline)
static void cliDump(char *cmdline)
{
int i, val = 0;
int i;
char buf[16];
float thr, roll, pitch, yaw;
uint32_t mask;
@ -531,17 +538,17 @@ static void cliDump(char *cmdline)
cliAux("");
// print out current motor mix
printf("mixer %s\r\n", mixerNames[cfg.mixerConfiguration - 1]);
printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
// print custom mix if exists
if (cfg.customMixer[0].throttle != 0.0f) {
if (mcfg.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_MOTORS; i++) {
if (cfg.customMixer[i].throttle == 0.0f)
if (mcfg.customMixer[i].throttle == 0.0f)
break;
thr = cfg.customMixer[i].throttle;
roll = cfg.customMixer[i].roll;
pitch = cfg.customMixer[i].pitch;
yaw = cfg.customMixer[i].yaw;
thr = mcfg.customMixer[i].throttle;
roll = mcfg.customMixer[i].roll;
pitch = mcfg.customMixer[i].pitch;
yaw = mcfg.customMixer[i].yaw;
printf("cmix %d", i + 1);
if (thr < 0)
printf(" ");
@ -575,7 +582,7 @@ static void cliDump(char *cmdline)
// print RC MAPPING
for (i = 0; i < 8; i++)
buf[cfg.rcmap[i]] = rcChannelLetters[i];
buf[mcfg.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0';
printf("map %s\r\n", buf);
@ -685,20 +692,20 @@ static void cliMap(char *cmdline)
}
uartPrint("Current assignment: ");
for (i = 0; i < 8; i++)
out[cfg.rcmap[i]] = rcChannelLetters[i];
out[mcfg.rcmap[i]] = rcChannelLetters[i];
out[i] = '\0';
printf("%s\r\n", out);
}
static void cliMixer(char *cmdline)
{
uint8_t i;
uint8_t len;
int i;
int len;
len = strlen(cmdline);
if (len == 0) {
printf("Current mixer: %s\r\n", mixerNames[cfg.mixerConfiguration - 1]);
printf("Current mixer: %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
return;
} else if (strncasecmp(cmdline, "list", len) == 0) {
uartPrint("Available mixers: ");
@ -717,17 +724,36 @@ static void cliMixer(char *cmdline)
break;
}
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
cfg.mixerConfiguration = i + 1;
mcfg.mixerConfiguration = i + 1;
printf("Mixer set to %s\r\n", mixerNames[i]);
break;
}
}
}
static void cliProfile(char *cmdline)
{
uint8_t len;
int i;
len = strlen(cmdline);
if (len == 0) {
printf("Current profile: %d\r\n", mcfg.current_profile);
return;
} else {
i = atoi(cmdline);
if (i >= 0 && i <= 2) {
mcfg.current_profile = i;
writeEEPROM(0, false);
cliProfile("");
}
}
}
static void cliSave(char *cmdline)
{
uartPrint("Saving...");
writeParams(0);
writeEEPROM(0, true);
uartPrint("\r\nRebooting...");
delay(10);
systemReset(false);
@ -856,11 +882,11 @@ static void cliStatus(char *cmdline)
if (sensors(SENSOR_ACC)) {
printf("ACCHW: %s", accNames[accHardware]);
if (accHardware == ACC_MPU6050)
printf(".%c", cfg.mpu6050_scale ? 'o' : 'n');
printf(".%c", mcfg.mpu6050_scale ? 'o' : 'n');
}
uartPrint("\r\n");
printf("Cycle Time: %d, I2C Errors: %d\r\n", cycleTime, i2cGetErrorCounter());
printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cGetErrorCounter(), sizeof(master_t));
}
static void cliVersion(char *cmdline)

205
src/config.c
View file

@ -9,10 +9,11 @@
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage
config_t cfg;
master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
static uint8_t EEPROM_CONF_VERSION = 34;
static uint8_t EEPROM_CONF_VERSION = 47;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -23,13 +24,13 @@ void parseRcChannels(const char *input)
for (c = input; *c; c++) {
s = strchr(rcChannelLetters, *c);
if (s)
cfg.rcmap[s - rcChannelLetters] = c - input;
mcfg.rcmap[s - rcChannelLetters] = c - input;
}
}
static uint8_t validEEPROM(void)
{
const config_t *temp = (const config_t *)FLASH_WRITE_ADDR;
const master_t *temp = (const master_t *)FLASH_WRITE_ADDR;
const uint8_t *p;
uint8_t chk = 0;
@ -38,11 +39,11 @@ static uint8_t validEEPROM(void)
return 0;
// check size and magic numbers
if (temp->size != sizeof(config_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
return 0;
// verify integrity of temporary copy
for (p = (const uint8_t *)temp; p < ((const uint8_t *)temp + sizeof(config_t)); p++)
for (p = (const uint8_t *)temp; p < ((const uint8_t *)temp + sizeof(master_t)); p++)
chk ^= *p;
// checksum failed
@ -57,8 +58,16 @@ void readEEPROM(void)
{
uint8_t i;
// Sanity check
if (!validEEPROM())
failureMode(10);
// Read flash
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t));
memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
// Copy current profile
if (mcfg.current_profile > 2) // sanity check
mcfg.current_profile = 0;
memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t));
for (i = 0; i < 6; i++)
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500;
@ -71,42 +80,64 @@ void readEEPROM(void)
if (tmp < 0)
y = cfg.thrMid8;
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; // [0;1000]
lookupThrottleRC[i] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
}
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
}
void writeParams(uint8_t b)
void writeEEPROM(uint8_t b, uint8_t updateProfile)
{
FLASH_Status status;
uint32_t i;
uint8_t chk = 0;
const uint8_t *p;
int tries = 0;
// prepare checksum/version constants
mcfg.version = EEPROM_CONF_VERSION;
mcfg.size = sizeof(master_t);
mcfg.magic_be = 0xBE;
mcfg.magic_ef = 0xEF;
mcfg.chk = 0;
// when updateProfile = true, we copy contents of cfg to global configuration. when false, only profile number is updated, and then that profile is loaded on readEEPROM()
if (updateProfile) {
// copy current in-memory profile to stored configuration
memcpy(&mcfg.profile[mcfg.current_profile], &cfg, sizeof(config_t));
}
cfg.version = EEPROM_CONF_VERSION;
cfg.size = sizeof(config_t);
cfg.magic_be = 0xBE;
cfg.magic_ef = 0xEF;
cfg.chk = 0;
// recalculate checksum before writing
for (p = (const uint8_t *)&cfg; p < ((const uint8_t *)&cfg + sizeof(config_t)); p++)
for (p = (const uint8_t *)&mcfg; p < ((const uint8_t *)&mcfg + sizeof(master_t)); p++)
chk ^= *p;
cfg.chk = chk;
mcfg.chk = chk;
// write it
retry:
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) {
for (i = 0; i < sizeof(config_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *) &cfg + i));
if (status != FLASH_COMPLETE)
break; // TODO: fail
for (i = 0; i < sizeof(master_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *)&mcfg + i));
if (status != FLASH_COMPLETE) {
FLASH_Lock();
tries++;
if (tries < 3)
goto retry;
else
break;
}
}
}
FLASH_Lock();
// Flash write failed - just die now
if (tries == 3 || !validEEPROM()) {
failureMode(10);
}
// re-read written data
readEEPROM();
if (b)
blinkLED(15, 20, 1);
@ -115,8 +146,11 @@ void writeParams(uint8_t b)
void checkFirstTime(bool reset)
{
// check the EEPROM integrity before resetting values
if (!validEEPROM() || reset)
if (!validEEPROM() || reset) {
resetConf();
// no need to memcpy profile again, we just did it in resetConf() above
writeEEPROM(0, false);
}
}
// Default settings
@ -125,13 +159,47 @@ static void resetConf(void)
int i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
// Clear all configuration
memset(&mcfg, 0, sizeof(master_t));
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
cfg.mixerConfiguration = MULTITYPE_QUADX;
mcfg.version = EEPROM_CONF_VERSION;
mcfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
featureSet(FEATURE_VBAT);
// global settings
mcfg.current_profile = 0; // default profile
mcfg.gyro_cmpf_factor = 400; // default MWC
mcfg.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
mcfg.accZero[0] = 0;
mcfg.accZero[1] = 0;
mcfg.accZero[2] = 0;
memcpy(&mcfg.align, default_align, sizeof(mcfg.align));
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
mcfg.moron_threshold = 32;
mcfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
mcfg.vbatscale = 110;
mcfg.vbatmaxcellvoltage = 43;
mcfg.vbatmincellvoltage = 33;
mcfg.power_adc_channel = 0;
mcfg.spektrum_hires = 0;
mcfg.midrc = 1500;
mcfg.mincheck = 1100;
mcfg.maxcheck = 1900;
mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// Motor/ESC/Servo
mcfg.minthrottle = 1150;
mcfg.maxthrottle = 1850;
mcfg.mincommand = 1000;
mcfg.motor_pwm_rate = 400;
mcfg.servo_pwm_rate = 50;
// gps/nav stuff
mcfg.gps_type = GPS_NMEA;
mcfg.gps_baudrate = 115200;
// serial (USART1) baudrate
mcfg.serial_baudrate = 115200;
// cfg.looptime = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
@ -141,82 +209,57 @@ static void resetConf(void)
cfg.D8[PITCH] = 23;
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45;
// cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 16;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 64;
cfg.I8[PIDALT] = 25;
cfg.D8[PIDALT] = 24;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
// cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
// cfg.D8[PIDPOS] = 0;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 70;
cfg.P8[PIDLEVEL] = 90;
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 20;
cfg.D8[PIDLEVEL] = 100;
cfg.P8[PIDMAG] = 40;
// cfg.P8[PIDVEL] = 0;
// cfg.I8[PIDVEL] = 0;
// cfg.D8[PIDVEL] = 0;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 90;
cfg.rcExpo8 = 65;
// cfg.rollPitchRate = 0;
// cfg.yawRate = 0;
// cfg.dynThrPID = 0;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
// cfg.thrExpo8 = 0;
cfg.thrExpo8 = 0;
// for (i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0;
// cfg.angleTrim[0] = 0;
// cfg.angleTrim[1] = 0;
// cfg.accZero[0] = 0;
// cfg.accZero[1] = 0;
// cfg.accZero[2] = 0;
// cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
memcpy(&cfg.align, default_align, sizeof(cfg.align));
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.angleTrim[0] = 0;
cfg.angleTrim[1] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
cfg.accz_deadband = 50;
cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42;
cfg.mpu6050_scale = 1; // fuck invensense
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
cfg.baro_cf = 0.985f;
cfg.moron_threshold = 32;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
// cfg.power_adc_channel = 0;
// Radio
parseRcChannels("AETR1234");
// cfg.deadband = 0;
// cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
// cfg.spektrum_hires = 0;
cfg.midrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
// cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
cfg.deadband = 0;
cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 40;
cfg.alt_hold_fast_change = 1;
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
// Motor/ESC/Servo
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
cfg.mincommand = 1000;
cfg.motor_pwm_rate = 400;
cfg.servo_pwm_rate = 50;
// servos
cfg.yaw_direction = 1;
cfg.tri_yaw_middle = 1500;
@ -247,23 +290,21 @@ static void resetConf(void)
cfg.gimbal_roll_mid = 1500;
// gps/nav stuff
cfg.gps_type = GPS_NMEA;
cfg.gps_baudrate = 115200;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
cfg.nav_slew_rate = 30;
cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
cfg.ap_mode = 40;
// custom mixer. clear by defaults.
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
mcfg.customMixer[i].throttle = 0.0f;
writeParams(0);
// copy default config into all 3 profiles
for (i = 0; i < 3; i++)
memcpy(&mcfg.profile[i], &cfg, sizeof(config_t));
}
bool sensors(uint32_t mask)
@ -288,25 +329,25 @@ uint32_t sensorsMask(void)
bool feature(uint32_t mask)
{
return cfg.enabledFeatures & mask;
return mcfg.enabledFeatures & mask;
}
void featureSet(uint32_t mask)
{
cfg.enabledFeatures |= mask;
mcfg.enabledFeatures |= mask;
}
void featureClear(uint32_t mask)
{
cfg.enabledFeatures &= ~(mask);
mcfg.enabledFeatures &= ~(mask);
}
void featureClearAll()
{
cfg.enabledFeatures = 0;
mcfg.enabledFeatures = 0;
}
uint32_t featureMask(void)
{
return cfg.enabledFeatures;
return mcfg.enabledFeatures;
}

1
src/drv_adc_fy90q.c
View file

@ -21,6 +21,7 @@ void adcSensorInit(sensor_t *acc, sensor_t *gyro)
gyro->init = adcDummyInit;
gyro->read = adcGyroRead;
gyro->align = adcGyroAlign;
gyro->scale = 1.0f;
acc_1G = 376;
}

13
src/drv_l3g4200d.c
View file

@ -29,7 +29,7 @@ static void l3g4200dInit(void);
static void l3g4200dRead(int16_t *gyroData);
static void l3g4200dAlign(int16_t *gyroData);
bool l3g4200dDetect(sensor_t *gyro)
bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
{
uint8_t deviceid;
@ -42,13 +42,12 @@ bool l3g4200dDetect(sensor_t *gyro)
gyro->init = l3g4200dInit;
gyro->read = l3g4200dRead;
gyro->align = l3g4200dAlign;
// 14.2857dps/lsb scalefactor
gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
return true;
}
void l3g4200dConfig(uint16_t lpf)
{
// default LPF is set to 32Hz
switch (lpf) {
default:
case 32:
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
break;
@ -63,7 +62,7 @@ void l3g4200dConfig(uint16_t lpf)
break;
}
i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
return true;
}
static void l3g4200dInit(void)

3
src/drv_l3g4200d.h
View file

@ -1,4 +1,3 @@
#pragma once
bool l3g4200dDetect(sensor_t * gyro);
void l3g4200dConfig(uint16_t lpf);
bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf);

13
src/drv_mpu3050.c
View file

@ -31,7 +31,7 @@ static void mpu3050Read(int16_t *gyroData);
static void mpu3050Align(int16_t *gyroData);
static void mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(sensor_t *gyro)
bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
{
bool ack;
@ -45,12 +45,10 @@ bool mpu3050Detect(sensor_t *gyro)
gyro->read = mpu3050Read;
gyro->align = mpu3050Align;
gyro->temperature = mpu3050ReadTemp;
// 16.4 dps/lsb scalefactor
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
return true;
}
void mpu3050Config(uint16_t lpf)
{
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = MPU3050_DLPF_256HZ;
@ -61,6 +59,7 @@ void mpu3050Config(uint16_t lpf)
case 98:
mpuLowPassFilter = MPU3050_DLPF_98HZ;
break;
default:
case 42:
mpuLowPassFilter = MPU3050_DLPF_42HZ;
break;
@ -72,7 +71,7 @@ void mpu3050Config(uint16_t lpf)
break;
}
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
return true;
}
static void mpu3050Init(void)

3
src/drv_mpu3050.h
View file

@ -1,4 +1,3 @@
#pragma once
bool mpu3050Detect(sensor_t *gyro);
void mpu3050Config(uint16_t lpf);
bool mpu3050Detect(sensor_t *gyro, uint16_t lpf);

62
src/drv_mpu6050.c
View file

@ -114,27 +114,23 @@
#define MPU_RA_WHO_AM_I 0x75
#define MPU6050_SMPLRT_DIV 0 //8000Hz
// #define MPU6050_DLPF_CFG 0 // 256Hz
#define MPU6050_DLPF_CFG 3 // 42Hz
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
#define MPU6050_LPF_256HZ 0
#define MPU6050_LPF_188HZ 1
#define MPU6050_LPF_98HZ 2
#define MPU6050_LPF_42HZ 3
#define MPU6050_LPF_20HZ 4
#define MPU6050_LPF_10HZ 5
#define MPU6050_LPF_5HZ 6
static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
static void mpu6050AccInit(void);
static void mpu6050AccRead(int16_t * accData);
static void mpu6050AccAlign(int16_t * accData);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050AccAlign(int16_t *accData);
static void mpu6050GyroInit(void);
static void mpu6050GyroRead(int16_t * gyroData);
static void mpu6050GyroAlign(int16_t * gyroData);
static void mpu6050GyroRead(int16_t *gyroData);
static void mpu6050GyroAlign(int16_t *gyroData);
#ifdef MPU6050_DMP
static void mpu6050DmpInit(void);
@ -145,7 +141,7 @@ int16_t dmpGyroData[3];
extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;
bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale)
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
{
bool ack;
uint8_t sig, rev;
@ -194,11 +190,39 @@ bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale)
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
gyro->align = mpu6050GyroAlign;
// 16.4 dps/lsb scalefactor
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
// give halfacc (old revision) back to system
if (scale)
*scale = mpuAccelHalf;
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = MPU6050_LPF_256HZ;
break;
case 188:
mpuLowPassFilter = MPU6050_LPF_188HZ;
break;
case 98:
mpuLowPassFilter = MPU6050_LPF_98HZ;
break;
default:
case 42:
mpuLowPassFilter = MPU6050_LPF_42HZ;
break;
case 20:
mpuLowPassFilter = MPU6050_LPF_20HZ;
break;
case 10:
mpuLowPassFilter = MPU6050_LPF_10HZ;
break;
case 5:
mpuLowPassFilter = MPU6050_LPF_5HZ;
break;
}
#ifdef MPU6050_DMP
mpu6050DmpInit();
#endif
@ -256,7 +280,7 @@ static void mpu6050GyroInit(void)
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.

2
src/drv_mpu6050.h
View file

@ -1,5 +1,5 @@
#pragma once
bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale);
bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf, uint8_t *scale);
void mpu6050DmpLoop(void);
void mpu6050DmpResetFifo(void);

37
src/gps.c
View file

@ -45,12 +45,12 @@ void gpsInit(uint32_t baudrate)
GPS_set_pids();
uart2Init(baudrate, GPS_NewData, false);
if (cfg.gps_type == GPS_UBLOX)
if (mcfg.gps_type == GPS_UBLOX)
offset = 0;
else if (cfg.gps_type == GPS_MTK)
else if (mcfg.gps_type == GPS_MTK)
offset = 4;
if (cfg.gps_type != GPS_NMEA) {
if (mcfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
uart2ChangeBaud(init_speed[i]);
switch (baudrate) {
@ -72,12 +72,12 @@ void gpsInit(uint32_t baudrate)
}
uart2ChangeBaud(baudrate);
if (cfg.gps_type == GPS_UBLOX) {
if (mcfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
uart2Write(ubloxInit[i]); // send ubx init binary
delay(4);
}
} else if (cfg.gps_type == GPS_MTK) {
} else if (mcfg.gps_type == GPS_MTK) {
gpsPrint("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // only GGA and RMC sentence
gpsPrint("$PMTK220,200*2C\r\n"); // 5 Hz update rate
}
@ -92,7 +92,7 @@ static void gpsPrint(const char *str)
{
while (*str) {
uart2Write(*str);
if (cfg.gps_type == GPS_UBLOX)
if (mcfg.gps_type == GPS_UBLOX)
delay(4);
str++;
}
@ -134,6 +134,29 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
int16_t last_velocity;
} LeadFilter_PARAM;
void leadFilter_clear(LeadFilter_PARAM *param)
{
param->last_velocity = 0;
}
int32_t leadFilter_getPosition(LeadFilter_PARAM *param, int32_t pos, int16_t vel, float lag_in_seconds)
{
int16_t accel_contribution = (vel - param->last_velocity) * lag_in_seconds * lag_in_seconds;
int16_t vel_contribution = vel * lag_in_seconds;
// store velocity for next iteration
param->last_velocity = vel;
return pos + vel_contribution + accel_contribution;
}
LeadFilter_PARAM xLeadFilter;
LeadFilter_PARAM yLeadFilter;
typedef struct {
float kP;
float kI;
@ -752,7 +775,7 @@ static uint8_t hex_c(uint8_t n)
static bool GPS_newFrame(char c)
{
switch (cfg.gps_type) {
switch (mcfg.gps_type) {
case GPS_NMEA: // NMEA
case GPS_MTK: // MTK outputs NMEA too
return GPS_NMEA_newFrame(c);

32
src/imu.c
View file

@ -5,11 +5,12 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
float accLPFVel[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int16_t sonarAlt; //to think about the unit
int16_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int16_t vario = 0; // variometer in cm/s
float magneticDeclination = 0.0f; // calculated at startup from config
float accVelScale;
@ -87,15 +88,15 @@ void computeIMU(void)
static int16_t gyroSmooth[3] = { 0, 0, 0 };
if (Smoothing[0] == 0) {
// initialize
Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
Smoothing[ROLL] = (mcfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (mcfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
}
@ -137,17 +138,10 @@ void computeIMU(void)
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)cfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
// #define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result (ITG-3200)
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
typedef struct fp_vector {
float X;
@ -225,7 +219,7 @@ static void getEstimatedAttitude(void)
uint32_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
scale = (currentT - previousT) * gyro.scale;
previousT = currentT;
// Initialization
@ -265,7 +259,7 @@ static void getEstimatedAttitude(void)
// To do that, we just skip filter, as EstV already rotated by Gyro
if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)cfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
if (sensors(SENSOR_MAG)) {
@ -356,7 +350,7 @@ int32_t isq(int32_t x)
return x * x;
}
void getEstimatedAltitude(void)
int getEstimatedAltitude(void)
{
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE_MAX];
@ -371,7 +365,7 @@ void getEstimatedAltitude(void)
float baroVel;
if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL)
return;
return 0;
dTime = currentTime - deadLine;
deadLine = currentTime;
@ -421,5 +415,7 @@ void getEstimatedAltitude(void)
// D
BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
debug[3] = BaroPID;
return 1;
}
#endif /* BARO */

25
src/main.c
View file

@ -48,17 +48,15 @@ int main(void)
readEEPROM();
// configure power ADC
if (cfg.power_adc_channel > 0 && (cfg.power_adc_channel == 1 || cfg.power_adc_channel == 9))
adc_params.powerAdcChannel = cfg.power_adc_channel;
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
cfg.power_adc_channel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
serialInit(cfg.serial_baudrate);
// We have these sensors
#ifndef FY90Q
// AfroFlight32
@ -70,7 +68,7 @@ int main(void)
mixerInit(); // this will set useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
@ -79,9 +77,9 @@ int main(void)
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = cfg.motor_pwm_rate;
pwm_params.servoPwmRate = cfg.servo_pwm_rate;
switch (cfg.power_adc_channel) {
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
@ -105,7 +103,7 @@ int main(void)
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
if (feature(FEATURE_GPS))
gpsInit(cfg.gps_baudrate);
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
@ -136,10 +134,13 @@ int main(void)
if (feature(FEATURE_VBAT))
batteryInit();
serialInit(mcfg.serial_baudrate);
previousTime = micros();
if (cfg.mixerConfiguration == MULTITYPE_GIMBAL)
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
// loopy
@ -151,6 +152,6 @@ int main(void)
void HardFault_Handler(void)
{
// fall out of the sky
writeAllMotors(cfg.mincommand);
writeAllMotors(mcfg.mincommand);
while (1);
}

52
src/mixer.c
View file

@ -7,6 +7,7 @@ int16_t motor[MAX_MOTORS];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
static motorMixer_t currentMixer[MAX_MOTORS];
static servoParam_t currentServo[MAX_SERVOS];
static const motorMixer_t mixerTri[] = {
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
@ -131,31 +132,38 @@ const mixer_t mixers[] = {
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
};
static const servoParam_t servoInit = {
SERVO_NORMAL, // direction
1500, // middle
1020, // min
2000, // max
};
void mixerInit(void)
{
int i;
// enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[cfg.mixerConfiguration].useServo;
useServo = mixers[mcfg.mixerConfiguration].useServo;
// if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT))
useServo = 1;
if (cfg.mixerConfiguration == MULTITYPE_CUSTOM) {
if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) {
// load custom mixer into currentMixer
for (i = 0; i < MAX_MOTORS; i++) {
// check if done
if (cfg.customMixer[i].throttle == 0.0f)
if (mcfg.customMixer[i].throttle == 0.0f)
break;
currentMixer[i] = cfg.customMixer[i];
currentMixer[i] = mcfg.customMixer[i];
numberMotor++;
}
} else {
numberMotor = mixers[cfg.mixerConfiguration].numberMotor;
numberMotor = mixers[mcfg.mixerConfiguration].numberMotor;
// copy motor-based mixers
if (mixers[cfg.mixerConfiguration].motor) {
if (mixers[mcfg.mixerConfiguration].motor) {
for (i = 0; i < numberMotor; i++)
currentMixer[i] = mixers[cfg.mixerConfiguration].motor[i];
currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i];
}
}
}
@ -168,12 +176,12 @@ void mixerLoadMix(int index)
index++;
// clear existing
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
mcfg.customMixer[i].throttle = 0.0f;
// do we have anything here to begin with?
if (mixers[index].motor != NULL) {
for (i = 0; i < mixers[index].numberMotor; i++)
cfg.customMixer[i] = mixers[index].motor[i];
mcfg.customMixer[i] = mixers[index].motor[i];
}
}
@ -182,7 +190,7 @@ void writeServos(void)
if (!useServo)
return;
switch (cfg.mixerConfiguration) {
switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI:
pwmWriteServo(0, servo[4]);
pwmWriteServo(1, servo[5]);
@ -298,7 +306,7 @@ void mixTable(void)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
// airplane / servo mixes
switch (cfg.mixerConfiguration) {
switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI:
servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
@ -321,8 +329,8 @@ void mixTable(void)
motor[0] = rcCommand[THROTTLE];
if (f.PASSTHRU_MODE) {
// do not use sensors for correction, simple 2 channel mixing
servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - cfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - cfg.midrc);
servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - cfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - cfg.midrc);
servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc);
servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.midrc);
} else {
// use sensors to correct (gyro only or gyro + acc)
servo[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL];
@ -338,9 +346,9 @@ void mixTable(void)
uint16_t aux[2] = { 0, 0 };
if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY))
aux[0] = rcData[AUX3] - cfg.midrc;
aux[0] = rcData[AUX3] - mcfg.midrc;
if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34))
aux[1] = rcData[AUX4] - cfg.midrc;
aux[1] = rcData[AUX4] - mcfg.midrc;
servo[0] = cfg.gimbal_pitch_mid + aux[0];
servo[1] = cfg.gimbal_roll_mid + aux[1];
@ -372,16 +380,16 @@ void mixTable(void)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) {
if (maxMotor > cfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - cfg.maxthrottle;
motor[i] = constrain(motor[i], cfg.minthrottle, cfg.maxthrottle);
if ((rcData[THROTTLE]) < cfg.mincheck) {
if (maxMotor > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - mcfg.maxthrottle;
motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
if ((rcData[THROTTLE]) < mcfg.mincheck) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = cfg.minthrottle;
motor[i] = mcfg.minthrottle;
else
motor[i] = cfg.mincommand;
motor[i] = mcfg.mincommand;
}
if (!f.ARMED)
motor[i] = cfg.mincommand;
motor[i] = mcfg.mincommand;
}
}

308
src/mw.c
View file

@ -21,6 +21,7 @@ int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // inter
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
uint16_t rssi; // range: [0;1023]
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
uint8_t dynP8[3], dynI8[3], dynD8[3];
@ -100,7 +101,7 @@ void annexCode(void)
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - cfg.midrc), 500);
tmp = min(abs(rcData[axis] - mcfg.midrc), 500);
if (axis != 2) { // ROLL & PITCH
if (cfg.deadband) {
if (tmp > cfg.deadband) {
@ -127,12 +128,12 @@ void annexCode(void)
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
if (rcData[axis] < mcfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000);
tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000);
tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
@ -152,7 +153,7 @@ void annexCode(void)
vbatRaw += vbatRawArray[i];
vbat = batteryAdcToVoltage(vbatRaw / 8);
}
if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
@ -214,9 +215,9 @@ uint16_t pwmReadRawRC(uint8_t chan)
{
uint16_t data;
data = pwmRead(cfg.rcmap[chan]);
data = pwmRead(mcfg.rcmap[chan]);
if (data < 750 || data > 2250)
data = cfg.midrc;
data = mcfg.midrc;
return data;
}
@ -242,9 +243,36 @@ void computeRC(void)
}
}
static void mwArm(void)
{
if (calibratingG == 0 && f.ACC_CALIBRATED) {
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
// TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
if (!f.ARMED) { // arm now!
f.ARMED = 1;
headFreeModeHold = heading;
}
} else if (!f.ARMED) {
blinkLED(2, 255, 1);
}
}
static void mwDisarm(void)
{
if (f.ARMED)
f.ARMED = 0;
}
static void mwVario(void)
{
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t stTmp = 0;
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta, deltaSum;
@ -258,6 +286,7 @@ void loop(void)
static uint32_t loopTime;
uint16_t auxState = 0;
int16_t prop;
static uint8_t GPSNavReset = 1;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
@ -273,36 +302,72 @@ void loop(void)
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[i] = mcfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = cfg.failsafe_throttle;
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
}
// end of failsafe routine - next change is made with RcOptions setting
if (rcData[THROTTLE] < cfg.mincheck) {
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > mcfg.mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < mcfg.maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
if (rcDelayCommand < 250)
rcDelayCommand++;
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (rcData[THROTTLE] < mcfg.mincheck) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && !f.ARMED) {
if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
if (rcOptions[BOXARM] && f.OK_TO_ARM)
mwArm();
else if (f.ARMED)
mwDisarm();
}
}
if (rcDelayCommand == 20) {
if (f.ARMED) { // actions during armed
// Disarm on throttle down + yaw
if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
mwDisarm();
// Disarm on roll (only when retarded_arm is enabled)
if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
mwDisarm();
} else { // actions during not armed
i = 0;
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
calibratingG = 1000;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
}
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (!f.ARMED && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
if (rcDelayCommand == 20) {
if (sensors(SENSOR_BARO))
calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
// Inflight ACC Calibration
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
@ -315,72 +380,61 @@ void loop(void)
}
}
}
} else if (cfg.activate[BOXARM] > 0) {
if (rcOptions[BOXARM] && f.OK_TO_ARM) {
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt == 0
f.ARMED = 1;
headFreeModeHold = heading;
} else if (f.ARMED)
f.ARMED = 0;
rcDelayCommand = 0;
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && f.ARMED) {
if (rcDelayCommand == 20)
f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ((rcData[YAW] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
// Multiple configuration profiles
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
mcfg.current_profile = i - 1;
writeEEPROM(0, false);
blinkLED(2, 40, i);
// TODO alarmArray[0] = i;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > cfg.maxcheck && !f.ARMED) {
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20)
// Arm via YAW
if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
mwArm();
// Arm via ROLL
else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
mwArm();
// Calibrating Acc
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = 400;
rcDelayCommand++;
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20)
f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > cfg.maxcheck) {
// Calibrating Mag
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
f.CALIBRATE_MAG = 1;
i = 0;
// Acc Trim
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
cfg.angleTrim[PITCH] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[PITCH] < cfg.mincheck) {
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
cfg.angleTrim[PITCH] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] > cfg.maxcheck) {
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
cfg.angleTrim[ROLL] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] < cfg.mincheck) {
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
cfg.angleTrim[ROLL] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else {
rcDelayCommand = 0;
i = 1;
}
if (i) {
writeEEPROM(1, false);
rcDelayCommand = 0; // allow autorepetition
}
}
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
@ -389,12 +443,13 @@ void loop(void)
}
}
// Check AUX switches
for (i = 0; i < 4; i++)
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
for (i = 0; i < CHECKBOXITEMS; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
// note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false
if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
@ -407,6 +462,7 @@ void loop(void)
}
if (rcOptions[BOXHORIZON]) {
f.ANGLE_MODE = 0;
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
@ -426,6 +482,7 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
@ -434,9 +491,20 @@ void loop(void)
errorAltitudeI = 0;
BaroPID = 0;
}
} else
} else {
f.BARO_MODE = 0;
}
// Vario signalling activate
if (feature(FEATURE_VARIO)) {
if (rcOptions[BOXVARIO]) {
if (!f.VARIO_MODE) {
f.VARIO_MODE = 1;
}
} else {
f.VARIO_MODE = 0;
}
}
}
#endif
#ifdef MAG
@ -446,8 +514,9 @@ void loop(void)
f.MAG_MODE = 1;
magHold = heading;
}
} else
} else {
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
@ -463,18 +532,21 @@ void loop(void)
if (sensors(SENSOR_GPS)) {
if (f.GPS_FIX && GPS_numSat >= 5) {
// if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
GPSNavReset = 0;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
@ -482,9 +554,19 @@ void loop(void)
}
} else {
f.GPS_HOLD_MODE = 0;
// both boxes are unselected here, nav is reset if not already done
if (GPSNavReset == 0) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
}
} else {
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
nav_mode = NAV_MODE_NONE;
}
}
if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1;
@ -492,47 +574,49 @@ void loop(void)
f.PASSTHRU_MODE = 0;
}
if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
f.HEADFREE_MODE = 0;
}
} else { // not in rc loop
static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
switch (taskOrder++ % 4) {
static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
if (taskOrder > 3)
taskOrder -= 4;
switch (taskOrder) {
case 0:
taskOrder++;
#ifdef MAG
if (sensors(SENSOR_MAG))
Mag_getADC();
#endif
if (sensors(SENSOR_MAG) && Mag_getADC())
break;
#endif
case 1:
taskOrder++;
#ifdef BARO
if (sensors(SENSOR_BARO))
Baro_update();
#endif
if (sensors(SENSOR_BARO) && Baro_update())
break;
#endif
case 2:
taskOrder++;
#ifdef BARO
if (sensors(SENSOR_BARO))
getEstimatedAltitude();
#endif
if (sensors(SENSOR_BARO) && getEstimatedAltitude())
break;
#endif
case 3:
taskOrder++;
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
Sonar_update();
debug[2] = sonarAlt;
}
#endif
break;
default:
taskOrder = 0;
if (feature(FEATURE_VARIO) && f.VARIO_MODE)
mwVario();
break;
}
}
currentTime = micros();
if (cfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + cfg.looptime;
if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + mcfg.looptime;
computeIMU();
// Measure loop rate just afer reading the sensors
@ -561,20 +645,44 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
if (cfg.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
f.BARO_MODE = 0; // so that a new althold reference is defined
errorAltitudeI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
if (abs(AltHoldCorr) > 500) {
AltHold += AltHoldCorr / 500;
AltHoldCorr %= 500;
}
errorAltitudeI = 0;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
}
}
#endif
if (sensors(SENSOR_GPS)) {
// Check that we really need to navigate ?
if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME) {
// If not. Reset nav loops and all nav related parameters
GPS_reset_nav();
} else {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) {
@ -595,17 +703,13 @@ void loop(void)
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
#ifdef LEVEL_PDF
PTermACC = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
error -= gyroData[axis];

150
src/mw.h
View file

@ -4,7 +4,7 @@
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
#define BARO_TAB_SIZE_MAX 48
#define VERSION 211
#define VERSION 220
#define LAT 0
#define LON 1
@ -77,29 +77,51 @@ enum {
};
enum {
BOXANGLE = 0,
BOXARM = 0,
BOXANGLE,
BOXHORIZON,
BOXBARO,
BOXVARIO,
BOXMAG,
BOXHEADFREE,
BOXHEADADJ,
BOXCAMSTAB,
BOXCAMTRIG,
BOXARM,
BOXGPSHOME,
BOXGPSHOLD,
BOXPASSTHRU,
BOXHEADFREE,
BOXBEEPERON,
BOXLEDMAX,
BOXLEDLOW,
BOXLLIGHTS,
BOXHEADADJ,
BOXCALIB,
BOXGOV,
BOXOSD,
CHECKBOXITEMS
};
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
#define PIT_LO (1 << (2 * PITCH))
#define PIT_CE (3 << (2 * PITCH))
#define PIT_HI (2 << (2 * PITCH))
#define YAW_LO (1 << (2 * YAW))
#define YAW_CE (3 << (2 * YAW))
#define YAW_HI (2 << (2 * YAW))
#define THR_LO (1 << (2 * THROTTLE))
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define SERVO_NORMAL (1)
#define SERVO_REVERSE (-1)
// Custom mixer data per motor
typedef struct motorMixer_t {
float throttle;
float roll;
@ -107,12 +129,20 @@ typedef struct motorMixer_t {
float yaw;
} motorMixer_t;
// Custom mixer configuration
typedef struct mixer_t {
uint8_t numberMotor;
uint8_t useServo;
const motorMixer_t *motor;
} mixer_t;
typedef struct servoParam_t {
int8_t direction; // servo direction
uint16_t middle; // servo middle
uint16_t min; // servo min
uint16_t max; // servo max
} servoParam_t;
enum {
ALIGN_GYRO = 0,
ALIGN_ACCEL = 1,
@ -120,14 +150,6 @@ enum {
};
typedef struct config_t {
uint8_t version;
uint16_t size;
uint8_t magic_be; // magic number, should be 0xBE
uint8_t mixerConfiguration;
uint32_t enabledFeatures;
uint16_t looptime; // imu loop time in us
uint8_t P8[PIDITEMS];
uint8_t I8[PIDITEMS];
uint8_t D8[PIDITEMS];
@ -141,57 +163,30 @@ typedef struct config_t {
uint8_t yawRate;
uint8_t dynThrPID;
int16_t accZero[3];
int16_t magZero[3];
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
int16_t angleTrim[2]; // accelerometer trim
// sensor-related stuff
int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t acc_lpf_for_velocity; // ACC lowpass for AccZ height hold
uint8_t accz_deadband; // ??
uint16_t gyro_lpf; // mpuX050 LPF setting (TODO make it work on L3GD as well)
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
uint8_t mpu6050_scale; // seems es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. fucking invenshit won't release chip IDs so I can't autodetect it.
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint16_t activate[CHECKBOXITEMS]; // activate switches
uint8_t vbatscale; // adjust this to match battery voltage to reported value
uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V)
uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9)
// Radio/ESC-related configuration
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-20
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
// Failsafe related configuration
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
// motor/esc/servo related stuff
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
int16_t servotrim[8]; // Adjust Servo MID Offset & Swash angles
int8_t servoreverse[8]; // Invert servos by setting -1
// mixer-related configuration
int8_t yaw_direction;
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
@ -205,6 +200,7 @@ typedef struct config_t {
uint16_t wing_right_min;
uint16_t wing_right_mid;
uint16_t wing_right_max;
int8_t pitch_direction_l; // left servo - pitch orientation
int8_t pitch_direction_r; // right servo - pitch orientation (opposite sign to pitch_direction_l if servos are mounted mirrored)
int8_t roll_direction_l; // left servo - roll orientation
@ -222,22 +218,71 @@ typedef struct config_t {
uint16_t gimbal_roll_mid; // gimbal roll servo neutral value
// gps-related stuff
uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ??
uint32_t gps_baudrate; // GPS baudrate
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz)
uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes
uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it
uint16_t nav_speed_min; // cm/sec
uint16_t nav_speed_max; // cm/sec
uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
} config_t;
// System-wide
typedef struct master_t {
uint8_t version;
uint16_t size;
uint8_t magic_be; // magic number, should be 0xBE
uint8_t mixerConfiguration;
uint32_t enabledFeatures;
uint16_t looptime; // imu loop time in us
motorMixer_t customMixer[MAX_MOTORS]; // custom mixtable
// motor/esc/servo related stuff
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
// global sensor-related stuff
int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint8_t mpu6050_scale; // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver.
int16_t accZero[3];
int16_t magZero[3];
// Battery/ADC stuff
uint8_t vbatscale; // adjust this to match battery voltage to reported value
uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V)
uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9)
// Radio/ESC-related configuration
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
// gps-related stuff
uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ??
uint32_t gps_baudrate; // GPS baudrate
// serial(uart1) baudrate
uint32_t serial_baudrate;
motorMixer_t customMixer[MAX_MOTORS]; // custom mixtable
config_t profile[3]; // 3 separate profiles
uint8_t current_profile; // currently loaded profile
uint8_t magic_ef; // magic number, should be 0xEF
uint8_t chk; // XOR checksum
} config_t;
} master_t;
typedef struct flags_t {
uint8_t OK_TO_ARM;
@ -255,6 +300,7 @@ typedef struct flags_t {
uint8_t GPS_FIX_HOME;
uint8_t SMALL_ANGLES_25;
uint8_t CALIBRATE_MAG;
uint8_t VARIO_MODE;
} flags_t;
extern int16_t gyroZero[3];
@ -272,6 +318,7 @@ extern uint32_t currentTime;
extern uint32_t previousTime;
extern uint16_t cycleTime;
extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
@ -281,12 +328,14 @@ extern int32_t EstAlt;
extern int32_t AltHold;
extern int16_t errorAltitudeI;
extern int16_t BaroPID;
extern int16_t vario;
extern int16_t headFreeModeHold;
extern int16_t zVelocity;
extern int16_t heading, magHold;
extern int16_t motor[MAX_MOTORS];
extern int16_t servo[8];
extern int16_t rcData[8];
extern uint16_t rssi; // range: [0;1023]
extern uint8_t vbat;
extern int16_t telemTemperature1; // gyro sensor temperature
extern int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
@ -310,6 +359,7 @@ extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern master_t mcfg;
extern config_t cfg;
extern flags_t f;
extern sensor_t acc;
@ -324,17 +374,17 @@ void imuInit(void);
void annexCode(void);
void computeIMU(void);
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat);
void getEstimatedAltitude(void);
int getEstimatedAltitude(void);
// Sensors
void sensorsAutodetect(void);
void batteryInit(void);
uint16_t batteryAdcToVoltage(uint16_t src);
void ACC_getADC(void);
void Baro_update(void);
int Baro_update(void);
void Gyro_getADC(void);
void Mag_init(void);
void Mag_getADC(void);
int Mag_getADC(void);
void Sonar_init(void);
void Sonar_update(void);
@ -353,7 +403,7 @@ void serialCom(void);
// Config
void parseRcChannels(const char *input);
void readEEPROM(void);
void writeParams(uint8_t b);
void writeEEPROM(uint8_t b, uint8_t updateProfile);
void checkFirstTime(bool reset);
bool sensors(uint32_t mask);
void sensorsSet(uint32_t mask);

109
src/sensors.c
View file

@ -2,6 +2,7 @@
#include "mw.h"
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
uint16_t calibratingG = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
int16_t heading, magHold;
@ -33,52 +34,52 @@ void sensorsAutodetect(void)
int16_t deg, min;
drv_adxl345_config_t acc_params;
bool haveMpu6k = false;
bool havel3g4200d = false;
// Autodetect gyro hardware. We have MPU3050 or MPU6050.
if (mpu6050Detect(&acc, &gyro, &cfg.mpu6050_scale)) {
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale)) {
// this filled up acc.* struct with init values
haveMpu6k = true;
} else if (l3g4200dDetect(&gyro)) {
havel3g4200d = true;
} else if (!mpu3050Detect(&gyro)) {
} else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
// well, we found our gyro
;
} else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
failureMode(3);
}
// Accelerometer. Fuck it. Let user break shit.
retry:
switch (cfg.acc_hardware) {
switch (mcfg.acc_hardware) {
case 0: // autodetect
case 1: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc))
accHardware = ACC_ADXL345;
if (cfg.acc_hardware == ACC_ADXL345)
if (mcfg.acc_hardware == ACC_ADXL345)
break;
; // fallthrough
case 2: // MPU6050
if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, &cfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (cfg.acc_hardware == ACC_MPU6050)
if (mcfg.acc_hardware == ACC_MPU6050)
break;
}
; // fallthrough
case 3: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (cfg.acc_hardware == ACC_MMA8452)
if (mcfg.acc_hardware == ACC_MMA8452)
break;
}
}
// Found anything? Check if user fucked up or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (cfg.acc_hardware > ACC_DEFAULT) {
if (mcfg.acc_hardware > ACC_DEFAULT) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
cfg.acc_hardware = ACC_DEFAULT;
mcfg.acc_hardware = ACC_DEFAULT;
goto retry;
} else {
// We're really screwed
@ -103,14 +104,6 @@ retry:
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
// todo: this is driver specific :(
if (havel3g4200d) {
l3g4200dConfig(cfg.gyro_lpf);
} else {
if (!haveMpu6k)
mpu3050Config(cfg.gyro_lpf);
}
#ifdef MAG
if (!hmc5883lDetect())
sensorsClear(SENSOR_MAG);
@ -127,7 +120,7 @@ uint16_t batteryAdcToVoltage(uint16_t src)
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 4095 = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
return (((src) * 3.3f) / 4095) * cfg.vbatscale;
return (((src) * 3.3f) / 4095) * mcfg.vbatscale;
}
void batteryInit(void)
@ -145,11 +138,11 @@ void batteryInit(void)
// autodetect cell count, going from 2S..6S
for (i = 2; i < 6; i++) {
if (voltage < i * cfg.vbatmaxcellvoltage)
if (voltage < i * mcfg.vbatmaxcellvoltage)
break;
}
batteryCellCount = i;
batteryWarningVoltage = i * cfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
}
// ALIGN_GYRO = 0,
@ -166,7 +159,7 @@ static void alignSensors(uint8_t type, int16_t *data)
tmp[2] = data[2];
for (i = 0; i < 3; i++) {
int8_t axis = cfg.align[type][i];
int8_t axis = mcfg.align[type][i];
if (axis > 0)
data[axis - 1] = tmp[i];
else
@ -188,16 +181,16 @@ static void ACC_Common(void)
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
cfg.accZero[axis] = 0;
mcfg.accZero[axis] = 0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
cfg.accZero[ROLL] = a[ROLL] / 400;
cfg.accZero[PITCH] = a[PITCH] / 400;
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
mcfg.accZero[ROLL] = a[ROLL] / 400;
mcfg.accZero[PITCH] = a[PITCH] / 400;
mcfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM
writeEEPROM(1, true); // write accZero in EEPROM
}
calibratingA--;
}
@ -208,9 +201,9 @@ static void ACC_Common(void)
static int16_t angleTrim_saved[2] = { 0, 0 };
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = cfg.accZero[ROLL];
accZero_saved[PITCH] = cfg.accZero[PITCH];
accZero_saved[YAW] = cfg.accZero[YAW];
accZero_saved[ROLL] = mcfg.accZero[ROLL];
accZero_saved[PITCH] = mcfg.accZero[PITCH];
accZero_saved[YAW] = mcfg.accZero[YAW];
angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
}
@ -223,7 +216,7 @@ static void ACC_Common(void)
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
cfg.accZero[axis] = 0;
mcfg.accZero[axis] = 0;
}
// all values are measured
if (InflightcalibratingA == 1) {
@ -231,9 +224,9 @@ static void ACC_Common(void)
AccInflightCalibrationMeasurementDone = 1;
toggleBeep = 2; // buzzer for indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred
cfg.accZero[ROLL] = accZero_saved[ROLL];
cfg.accZero[PITCH] = accZero_saved[PITCH];
cfg.accZero[YAW] = accZero_saved[YAW];
mcfg.accZero[ROLL] = accZero_saved[ROLL];
mcfg.accZero[PITCH] = accZero_saved[PITCH];
mcfg.accZero[YAW] = accZero_saved[YAW];
cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
}
@ -242,25 +235,25 @@ static void ACC_Common(void)
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1) { // the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
cfg.accZero[ROLL] = b[ROLL] / 50;
cfg.accZero[PITCH] = b[PITCH] / 50;
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
mcfg.accZero[ROLL] = b[ROLL] / 50;
mcfg.accZero[PITCH] = b[PITCH] / 50;
mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM
writeEEPROM(1, true); // write accZero in EEPROM
}
}
accADC[ROLL] -= cfg.accZero[ROLL];
accADC[PITCH] -= cfg.accZero[PITCH];
accADC[YAW] -= cfg.accZero[YAW];
accADC[ROLL] -= mcfg.accZero[ROLL];
accADC[PITCH] -= mcfg.accZero[PITCH];
accADC[YAW] -= mcfg.accZero[YAW];
}
void ACC_getADC(void)
{
acc.read(accADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (cfg.align[ALIGN_ACCEL][0])
if (mcfg.align[ALIGN_ACCEL][0])
alignSensors(ALIGN_ACCEL, accADC);
else
acc.align(accADC);
@ -269,14 +262,14 @@ void ACC_getADC(void)
}
#ifdef BARO
void Baro_update(void)
int Baro_update(void)
{
static uint32_t baroDeadline = 0;
static uint8_t state = 0;
int32_t pressure;
if ((int32_t)(currentTime - baroDeadline) < 0)
return;
return 0;
baroDeadline = currentTime;
@ -303,6 +296,8 @@ void Baro_update(void)
baroDeadline += baro.repeat_delay;
break;
}
return 1;
}
#endif /* BARO */
@ -364,7 +359,7 @@ static void GYRO_Common(void)
if (calibratingG == 1) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (cfg.moron_threshold && dev > cfg.moron_threshold) {
if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
calibratingG = 1000;
devClear(&var[0]);
devClear(&var[1]);
@ -391,7 +386,7 @@ void Gyro_getADC(void)
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (cfg.align[ALIGN_GYRO][0])
if (mcfg.align[ALIGN_GYRO][0])
alignSensors(ALIGN_GYRO, gyroADC);
else
gyro.align(gyroADC);
@ -491,7 +486,7 @@ void Mag_init(void)
magInit = 1;
}
void Mag_getADC(void)
int Mag_getADC(void)
{
static uint32_t t, tCal = 0;
static int16_t magZeroTempMin[3];
@ -499,7 +494,7 @@ void Mag_getADC(void)
uint32_t axis;
if ((int32_t)(currentTime - t) < 0)
return; //each read is spaced by 100ms
return 0; //each read is spaced by 100ms
t = currentTime + 100000;
// Read mag sensor
@ -512,7 +507,7 @@ void Mag_getADC(void)
if (f.CALIBRATE_MAG) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
cfg.magZero[axis] = 0;
mcfg.magZero[axis] = 0;
magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis];
}
@ -520,9 +515,9 @@ void Mag_getADC(void)
}
if (magInit) { // we apply offset only once mag calibration is done
magADC[ROLL] -= cfg.magZero[ROLL];
magADC[PITCH] -= cfg.magZero[PITCH];
magADC[YAW] -= cfg.magZero[YAW];
magADC[ROLL] -= mcfg.magZero[ROLL];
magADC[PITCH] -= mcfg.magZero[PITCH];
magADC[YAW] -= mcfg.magZero[YAW];
}
if (tCal != 0) {
@ -537,10 +532,12 @@ void Mag_getADC(void)
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++)
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
writeParams(1);
mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
writeEEPROM(1, true);
}
}
return 1;
}
#endif

293
src/serial.c
View file

@ -3,68 +3,133 @@
// Multiwii Serial Protocol 0
#define MSP_VERSION 0
#define PLATFORM_32BIT 0x80000000
#define PLATFORM_32BIT ((uint32_t)1 << 31)
#define MSP_IDENT 100 //out message multitype + version
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation
#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
#define MSP_RAW_IMU 102 //out message 9 DOF
#define MSP_SERVO 103 //out message 8 servos
#define MSP_MOTOR 104 //out message 8 motors
#define MSP_RC 105 //out message 8 rc chan
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed
#define MSP_RC 105 //out message 8 rc chan and more
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
#define MSP_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
#define MSP_ALTITUDE 109 //out message 1 altitude
#define MSP_BAT 110 //out message vbat, powermetersum
#define MSP_ALTITUDE 109 //out message altitude, variometer
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID 112 //out message up to 16 P I D (8 are used)
#define MSP_BOX 113 //out message up to 16 checkbox (11 are used)
#define MSP_MISC 114 //out message powermeter trig + 8 free for future use
#define MSP_PID 112 //out message P I D coeff (9 are used currently)
#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
#define MSP_MISC 114 //out message powermeter trig
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
#define MSP_BOXNAMES 116 //out message the aux switch names
#define MSP_PIDNAMES 117 //out message the PID names
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
#define MSP_SET_PID 202 //in message up to 16 P I D (8 are used)
#define MSP_SET_BOX 203 //in message up to 16 checkbox (11 are used)
#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_ACC_CALIBRATION 205 //in message no param
#define MSP_MAG_CALIBRATION 206 //in message no param
#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
#define MSP_RESET_CONF 208 //in message no param
#define MSP_WP_SET 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
#define MSP_SET_HEAD 211 //in message define a new heading hold direction
// #define MSP_BIND 240 //in message no param
#define MSP_EEPROM_WRITE 250 //in message no param
#define MSP_DEBUGMSG 253 //out message debug string buffer
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
// Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID
#define INBUF_SIZE 64
struct box_t {
const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name
const uint8_t permanentId; //
} boxes[] = {
{ BOXARM, "ARM;", 0 },
{ BOXANGLE, "ANGLE;", 1 },
{ BOXHORIZON, "HORIZON;", 2 },
{ BOXBARO, "BARO;", 3 },
{ BOXVARIO, "VARIO;", 4 },
{ BOXMAG, "MAG;", 5 },
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXCAMTRIG, "CAMTRIG;", 9 },
{ BOXGPSHOME, "GPS HOME;", 10 },
{ BOXGPSHOLD, "GPS HOLD;", 11 },
{ BOXPASSTHRU, "PASSTHRU;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDMAX, "LEDMAX;", 14 },
{ BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLLIGHTS, "LLIGHTS;", 16 },
{ BOXCALIB, "CALIB;", 17 },
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ CHECKBOXITEMS, NULL, 0xFF }
};
// this is calculated at startup based on enabled features.
static uint8_t availableBoxes[CHECKBOXITEMS];
// this is the number of filled indexes in above array
static uint8_t numberBoxItems = 0;
static const char boxnames[] =
"ARM;"
"ANGLE;"
"HORIZON;"
"BARO;"
"VARIO;"
"MAG;"
"HEADFREE;"
"HEADADJ;"
"CAMSTAB;"
"CAMTRIG;"
"ARM;"
"GPS HOME;"
"GPS HOLD;"
"PASSTHRU;"
"HEADFREE;"
"BEEPER;"
"LEDMAX;"
"LEDLOW;"
"LLIGHTS;"
"HEADADJ;";
"CALIB;"
"GOVERNOR;"
"OSD SW;";
const uint8_t boxids[] = { // permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function.
0, // "ARM;"
1, // "ANGLE;"
2, // "HORIZON;"
3, // "BARO;"
4, // "VARIO;"
5, // "MAG;"
6, // "HEADFREE;"
7, // "HEADADJ;"
8, // "CAMSTAB;"
9, // "CAMTRIG;"
10, // "GPS HOME;"
11, // "GPS HOLD;"
12, // "PASSTHRU;"
13, // "BEEPER;"
14, // "LEDMAX;"
15, // "LEDLOW;"
16, // "LLIGHTS;"
17, // "CALIB;"
18, // "GOVERNOR;"
19, // "OSD_SWITCH;"
};
static const char pidnames[] =
"ROLL;"
@ -169,15 +234,68 @@ void serializeNames(const char *s)
serialize8(*c);
}
void serializeBoxNamesReply(void)
{
char buf[256]; // no fucking idea
char *c;
int i, j;
memset(buf, 0, sizeof(buf));
for (i = 0; i < CHECKBOXITEMS; i++) {
for (j = 0; j < numberBoxItems; j++) {
if (boxes[i].boxIndex == availableBoxes[j])
strcat(buf, boxes[i].boxName);
}
}
headSerialReply(strlen(buf));
for (c = buf; *c; c++)
serialize8(*c);
}
void serialInit(uint32_t baudrate)
{
int idx;
uartInit(baudrate);
// calculate used boxes based on features and fill availableBoxes[] array
memset(availableBoxes, 0xFF, sizeof(availableBoxes));
idx = 0;
availableBoxes[idx++] = BOXARM;
if (sensors(SENSOR_ACC)) {
availableBoxes[idx++] = BOXANGLE;
availableBoxes[idx++] = BOXHORIZON;
}
if (sensors(SENSOR_BARO)) {
availableBoxes[idx++] = BOXBARO;
if (feature(FEATURE_VARIO))
availableBoxes[idx++] = BOXVARIO;
}
if (sensors(SENSOR_MAG)) {
availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ;
}
if (feature(FEATURE_SERVO_TILT))
availableBoxes[idx++] = BOXCAMSTAB;
if (feature(FEATURE_GPS) && sensors(SENSOR_GPS)) {
availableBoxes[idx++] = BOXGPSHOME;
availableBoxes[idx++] = BOXGPSHOLD;
}
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE)
availableBoxes[idx++] = BOXPASSTHRU;
availableBoxes[idx++] = BOXBEEPERON;
if (feature(FEATURE_INFLIGHT_ACC_CAL))
availableBoxes[idx++] = BOXCALIB;
numberBoxItems = idx;
}
static void evaluateCommand(void)
{
uint32_t i;
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
switch (cmdMSP) {
case MSP_SET_RAW_RC:
@ -209,8 +327,8 @@ static void evaluateCommand(void)
headSerialReply(0);
break;
case MSP_SET_BOX:
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = read16();
for (i = 0; i < numberBoxItems; i++)
cfg.activate[availableBoxes[i]] = read16();
headSerialReply(0);
break;
case MSP_SET_RC_TUNING:
@ -226,22 +344,46 @@ static void evaluateCommand(void)
case MSP_SET_MISC:
headSerialReply(0);
break;
case MSP_SELECT_SETTING:
if (!f.ARMED) {
mcfg.current_profile = read8();
if (mcfg.current_profile > 2)
mcfg.current_profile = 0;
// this writes new profile index and re-reads it
writeEEPROM(0, false);
}
headSerialReply(0);
break;
case MSP_SET_HEAD:
magHold = read16();
headSerialReply(0);
break;
case MSP_IDENT:
headSerialReply(7);
serialize8(VERSION); // multiwii version
serialize8(cfg.mixerConfiguration); // type of multicopter
serialize8(mcfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability"
break;
case MSP_STATUS:
headSerialReply(10);
headSerialReply(11);
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON |
rcOptions[BOXLEDMAX] << BOXLEDMAX |
rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
rcOptions[BOXVARIO] << BOXVARIO |
rcOptions[BOXCALIB] << BOXCALIB |
rcOptions[BOXGOV] << BOXGOV |
rcOptions[BOXOSD] << BOXOSD |
f.ARMED << BOXARM);
serialize8(mcfg.current_profile);
break;
case MSP_RAW_IMU:
headSerialReply(18);
@ -268,13 +410,14 @@ static void evaluateCommand(void)
serialize16(rcData[i]);
break;
case MSP_RAW_GPS:
headSerialReply(14);
headSerialReply(16);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
serialize16(GPS_ground_course);
break;
case MSP_COMP_GPS:
headSerialReply(5);
@ -290,13 +433,15 @@ static void evaluateCommand(void)
serialize16(headFreeModeHold);
break;
case MSP_ALTITUDE:
headSerialReply(4);
headSerialReply(6);
serialize32(EstAlt);
serialize16(vario);
break;
case MSP_BAT:
headSerialReply(3);
case MSP_ANALOG:
headSerialReply(5);
serialize8(vbat);
serialize16(0); // power meter trash
serialize16(rssi);
break;
case MSP_RC_TUNING:
headSerialReply(7);
@ -316,19 +461,24 @@ static void evaluateCommand(void)
serialize8(cfg.D8[i]);
}
break;
case MSP_BOX:
headSerialReply(2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++)
serialize16(cfg.activate[i]);
break;
case MSP_BOXNAMES:
headSerialReply(sizeof(boxnames) - 1);
serializeNames(boxnames);
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_BOX:
headSerialReply(2 * numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize16(cfg.activate[availableBoxes[i]]);
break;
case MSP_BOXNAMES:
// headSerialReply(sizeof(boxnames) - 1);
serializeBoxNamesReply();
break;
case MSP_BOXIDS:
headSerialReply(numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize8(availableBoxes[i]);
break;
case MSP_MISC:
headSerialReply(2);
serialize16(0); // intPowerTrigger1
@ -340,54 +490,85 @@ static void evaluateCommand(void)
break;
case MSP_WP:
wp_no = read8(); // get the wp number
headSerialReply(12);
headSerialReply(18);
if (wp_no == 0) {
serialize8(0); // wp0
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
lat = GPS_home[LAT];
lon = GPS_home[LON];
} else if (wp_no == 16) {
serialize8(16); // wp16
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
lat = GPS_hold[LAT];
lon = GPS_hold[LON];
}
serialize8(wp_no);
serialize32(lat);
serialize32(lon);
serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
serialize16(0); // heading will come here (deg)
serialize16(0); // time to stay (ms) will come here
serialize8(0); // nav flag will come here
break;
case MSP_SET_WP:
wp_no = read8(); //get the wp number
lat = read32();
lon = read32();
alt = read32(); // to set altitude (cm)
read16(); // future: to set heading (deg)
read16(); // future: to set time to stay (ms)
read8(); // future: to set nav flag
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
f.GPS_FIX_HOME = 1;
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
} else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
GPS_hold[LAT] = lat;
GPS_hold[LON] = lon;
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
nav_mode = NAV_MODE_WP;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
}
headSerialReply(0);
break;
case MSP_RESET_CONF:
if (!f.ARMED)
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
if (!f.ARMED)
calibratingA = 400;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
if (!f.ARMED)
f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
case MSP_EEPROM_WRITE:
writeParams(0);
writeEEPROM(0, true);
headSerialReply(0);
break;
case MSP_ACC_TRIM:
headSerialReply(4);
serialize16(cfg.angleTrim[PITCH]);
serialize16(cfg.angleTrim[ROLL]);
break;
case MSP_DEBUG:
headSerialReply(8);
for (i = 0; i < 4; i++)
serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
// Additional commands that are not compatible with MultiWii
case MSP_ACC_TRIM:
headSerialReply(4);
serialize16(cfg.angleTrim[PITCH]);
serialize16(cfg.angleTrim[ROLL]);
break;
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;

10
src/spektrum.c
View file

@ -17,7 +17,7 @@ extern int16_t failsafeCnt;
void spektrumInit(void)
{
if (cfg.spektrum_hires) {
if (mcfg.spektrum_hires) {
// 11 bit frames
spek_chan_shift = 3;
spek_chan_mask = 0x07;
@ -75,12 +75,12 @@ uint16_t spektrumReadRawRC(uint8_t chan)
}
if (chan >= SPEK_MAX_CHANNEL || !spekDataIncoming) {
data = cfg.midrc;
data = mcfg.midrc;
} else {
if (cfg.spektrum_hires)
data = 988 + (spekChannelData[cfg.rcmap[chan]] >> 1); // 2048 mode
if (mcfg.spektrum_hires)
data = 988 + (spekChannelData[mcfg.rcmap[chan]] >> 1); // 2048 mode
else
data = 988 + spekChannelData[cfg.rcmap[chan]]; // 1024 mode
data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode
}
return data;

2
src/telemetry.c
View file

@ -209,7 +209,7 @@ void initTelemetry(bool State)
if (State)
serialInit(9600);
else
serialInit(cfg.serial_baudrate);
serialInit(mcfg.serial_baudrate);
telemetryEnabled = State;
}
}