englebert
/
inav-simplefc
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

begin initial merge of 2.2 features 

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
master
timecop@gmail.com 12 years ago
parent
bba9bc291f
commit
491b3627f6
20 changed files with 1009 additions and 559 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      src/board.h
  2. 152
      src/cli.c
  3. 205
      src/config.c
  4. 1
      src/drv_adc_fy90q.c
  5. 37
      src/drv_l3g4200d.c
  6. 3
      src/drv_l3g4200d.h
  7. 13
      src/drv_mpu3050.c
  8. 3
      src/drv_mpu3050.h
  9. 64
      src/drv_mpu6050.c
  10. 2
      src/drv_mpu6050.h
  11. 37
      src/gps.c
  12. 44
      src/imu.c
  13. 29
      src/main.c
  14. 52
      src/mixer.c
  15. 338
      src/mw.c
  16. 150
      src/mw.h
  17. 115
      src/sensors.c
  18. 301
      src/serial.c
  19. 10
      src/spektrum.c
  20. 2
      src/telemetry.c

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

152
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;
}

37
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,28 +42,27 @@ 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) {
case 32:
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
break;
case 54:
mpuLowPassFilter = L3G4200D_DLPF_54HZ;
break;
case 78:
mpuLowPassFilter = L3G4200D_DLPF_78HZ;
break;
case 93:
mpuLowPassFilter = L3G4200D_DLPF_93HZ;
break;
default:
case 32:
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
break;
case 54:
mpuLowPassFilter = L3G4200D_DLPF_54HZ;
break;
case 78:
mpuLowPassFilter = L3G4200D_DLPF_78HZ;
break;
case 93:
mpuLowPassFilter = L3G4200D_DLPF_93HZ;
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);

64
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);

44
src/imu.c
View File

@ -4,12 +4,13 @@
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
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int32_t BaroAlt;
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)) {
@ -282,7 +276,7 @@ static void getEstimatedAttitude(void)
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
#if INACCURATE
@ -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 */

29
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
@ -135,11 +133,14 @@ int main(void)
// Check battery type/voltage
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;
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;
}
}

338
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
// ------------------ 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;
if (rcData[THROTTLE] < cfg.mincheck) {
// 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 (rcDelayCommand == 20) {
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) {
cfg.angleTrim[PITCH] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[PITCH] < cfg.mincheck) {
cfg.angleTrim[PITCH] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] > cfg.maxcheck) {
cfg.angleTrim[ROLL] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] < cfg.mincheck) {
cfg.angleTrim[ROLL] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else {
rcDelayCommand = 0;
// 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;
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
cfg.angleTrim[PITCH] -= 2;
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
cfg.angleTrim[ROLL] += 2;
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
cfg.angleTrim[ROLL] -= 2;
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,8 +491,19 @@ 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
@ -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,26 +532,39 @@ 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 (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
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]);
nav_mode = NAV_MODE_POSHOLD;
}
} 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_HOLD_MODE = 0;
}
} else {
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
nav_mode = NAV_MODE_NONE;
}
}
@ -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();
if (sensors(SENSOR_MAG) && Mag_getADC())
break;
#endif
break;
case 1:
taskOrder++;
#ifdef BARO
if (sensors(SENSOR_BARO))
Baro_update();
if (sensors(SENSOR_BARO) && Baro_update())
break;
#endif
break;
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) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
f.BARO_MODE = 0; // so that a new althold reference is defined
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) {
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;
}
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);

115
src/sensors.c
View File

@ -1,9 +1,10 @@
#include "board.h"
#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 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.
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
int16_t heading, magHold;
extern uint16_t InflightcalibratingA;
@ -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,15 +486,15 @@ 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];
static int16_t magZeroTempMax[3];
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

301
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
#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 MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#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 |
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);
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.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,18 +461,23 @@ static void evaluateCommand(void)
serialize8(cfg.D8[i]);
}
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_BOX:
headSerialReply(2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++)
serialize16(cfg.activate[i]);
headSerialReply(2 * numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize16(cfg.activate[availableBoxes[i]]);
break;
case MSP_BOXNAMES:
headSerialReply(sizeof(boxnames) - 1);
serializeNames(boxnames);
// headSerialReply(sizeof(boxnames) - 1);
serializeBoxNamesReply();
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
case MSP_BOXIDS:
headSerialReply(numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize8(availableBoxes[i]);
break;
case MSP_MISC:
headSerialReply(2);
@ -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:
checkFirstTime(true);
if (!f.ARMED)
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
calibratingA = 400;
if (!f.ARMED)
calibratingA = 400;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
f.CALIBRATE_MAG = 1;
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
// 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;
}
}

Write Preview
Loading…
Cancel
Save