mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
PWM6+7/TIM3_CH3+4/PB0+PB1 Update software serial to monitor serial pins for signal changes instead of periodically sampling pin signals. When reading the data the timer used is syncronized to the falling edge of the start bit which allows for better syncronisation at higher speeds. The code has been tested OK from 1200 baud to 19200. 38400 baud was tested and partially usable but has been disabled because there are too many transmit and receive errors, especially when transmitting and receiving at the same time. Due to the way a single timer is used for transmitting and receiving, if data comes in while transmitting the system may incorrectly transmit a short or long bit. However at 19200 and below this didn't cause a problem in the limited testing I performed.
363 lines
10 KiB
C
Executable file
363 lines
10 KiB
C
Executable file
#include "board.h"
|
|
#include "mw.h"
|
|
#include <string.h>
|
|
|
|
#ifndef FLASH_PAGE_COUNT
|
|
#define FLASH_PAGE_COUNT 128
|
|
#endif
|
|
|
|
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
|
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
|
|
|
|
master_t mcfg; // master config struct with data independent from profiles
|
|
config_t cfg; // profile config struct
|
|
const char rcChannelLetters[] = "AERT1234";
|
|
|
|
static const uint8_t EEPROM_CONF_VERSION = 59;
|
|
static uint32_t enabledSensors = 0;
|
|
static void resetConf(void);
|
|
|
|
void parseRcChannels(const char *input)
|
|
{
|
|
const char *c, *s;
|
|
|
|
for (c = input; *c; c++) {
|
|
s = strchr(rcChannelLetters, *c);
|
|
if (s)
|
|
mcfg.rcmap[s - rcChannelLetters] = c - input;
|
|
}
|
|
}
|
|
|
|
static uint8_t validEEPROM(void)
|
|
{
|
|
const master_t *temp = (const master_t *)FLASH_WRITE_ADDR;
|
|
const uint8_t *p;
|
|
uint8_t chk = 0;
|
|
|
|
// check version number
|
|
if (EEPROM_CONF_VERSION != temp->version)
|
|
return 0;
|
|
|
|
// check size and magic numbers
|
|
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(master_t)); p++)
|
|
chk ^= *p;
|
|
|
|
// checksum failed
|
|
if (chk != 0)
|
|
return 0;
|
|
|
|
// looks good, let's roll!
|
|
return 1;
|
|
}
|
|
|
|
void readEEPROM(void)
|
|
{
|
|
uint8_t i;
|
|
|
|
// Sanity check
|
|
if (!validEEPROM())
|
|
failureMode(10);
|
|
|
|
// Read flash
|
|
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 < PITCH_LOOKUP_LENGTH; i++)
|
|
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500;
|
|
|
|
for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
|
|
int16_t tmp = 10 * i - cfg.thrMid8;
|
|
uint8_t y = 1;
|
|
if (tmp > 0)
|
|
y = 100 - cfg.thrMid8;
|
|
if (tmp < 0)
|
|
y = cfg.thrMid8;
|
|
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10;
|
|
lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
|
|
}
|
|
|
|
setPIDController(cfg.pidController);
|
|
gpsSetPIDs();
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
// recalculate checksum before writing
|
|
for (p = (const uint8_t *)&mcfg; p < ((const uint8_t *)&mcfg + sizeof(master_t)); p++)
|
|
chk ^= *p;
|
|
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(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);
|
|
}
|
|
|
|
void checkFirstTime(bool reset)
|
|
{
|
|
// check the EEPROM integrity before resetting values
|
|
if (!validEEPROM() || reset) {
|
|
resetConf();
|
|
// no need to memcpy profile again, we just did it in resetConf() above
|
|
writeEEPROM(0, false);
|
|
}
|
|
}
|
|
|
|
// Default settings
|
|
static void resetConf(void)
|
|
{
|
|
int i;
|
|
int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 };
|
|
|
|
// Clear all configuration
|
|
memset(&mcfg, 0, sizeof(master_t));
|
|
memset(&cfg, 0, sizeof(config_t));
|
|
|
|
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 = 600; // default MWC
|
|
mcfg.gyro_cmpfm_factor = 250; // 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;
|
|
mcfg.gyro_align = ALIGN_DEFAULT;
|
|
mcfg.acc_align = ALIGN_DEFAULT;
|
|
mcfg.mag_align = ALIGN_DEFAULT;
|
|
mcfg.board_align_roll = 0;
|
|
mcfg.board_align_pitch = 0;
|
|
mcfg.board_align_yaw = 0;
|
|
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
|
|
mcfg.max_angle_inclination = 500; // 50 degrees
|
|
mcfg.yaw_control_direction = 1;
|
|
mcfg.moron_threshold = 32;
|
|
mcfg.vbatscale = 110;
|
|
mcfg.vbatmaxcellvoltage = 43;
|
|
mcfg.vbatmincellvoltage = 33;
|
|
mcfg.power_adc_channel = 0;
|
|
mcfg.serialrx_type = 0;
|
|
mcfg.telemetry_softserial = 0;
|
|
mcfg.telemetry_switch = 0;
|
|
mcfg.midrc = 1500;
|
|
mcfg.mincheck = 1100;
|
|
mcfg.maxcheck = 1900;
|
|
mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right
|
|
mcfg.flaps_speed = 0;
|
|
mcfg.fixedwing_althold_dir = 1;
|
|
// Motor/ESC/Servo
|
|
mcfg.minthrottle = 1150;
|
|
mcfg.maxthrottle = 1850;
|
|
mcfg.mincommand = 1000;
|
|
mcfg.deadband3d_low = 1406;
|
|
mcfg.deadband3d_high = 1514;
|
|
mcfg.neutral3d = 1460;
|
|
mcfg.deadband3d_throttle = 50;
|
|
mcfg.motor_pwm_rate = 400;
|
|
mcfg.servo_pwm_rate = 50;
|
|
// gps/nav stuff
|
|
mcfg.gps_type = GPS_NMEA;
|
|
mcfg.gps_baudrate = 0;
|
|
// serial (USART1) baudrate
|
|
mcfg.serial_baudrate = 115200;
|
|
mcfg.softserial_baudrate = 19200;
|
|
mcfg.softserial_1_inverted = 0;
|
|
mcfg.softserial_2_inverted = 0;
|
|
mcfg.looptime = 3500;
|
|
mcfg.rssi_aux_channel = 0;
|
|
|
|
cfg.pidController = 0;
|
|
cfg.P8[ROLL] = 40;
|
|
cfg.I8[ROLL] = 30;
|
|
cfg.D8[ROLL] = 23;
|
|
cfg.P8[PITCH] = 40;
|
|
cfg.I8[PITCH] = 30;
|
|
cfg.D8[PITCH] = 23;
|
|
cfg.P8[YAW] = 85;
|
|
cfg.I8[YAW] = 45;
|
|
cfg.D8[YAW] = 0;
|
|
cfg.P8[PIDALT] = 50;
|
|
cfg.I8[PIDALT] = 0;
|
|
cfg.D8[PIDALT] = 0;
|
|
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
|
|
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] = 90;
|
|
cfg.I8[PIDLEVEL] = 10;
|
|
cfg.D8[PIDLEVEL] = 100;
|
|
cfg.P8[PIDMAG] = 40;
|
|
cfg.P8[PIDVEL] = 120;
|
|
cfg.I8[PIDVEL] = 45;
|
|
cfg.D8[PIDVEL] = 1;
|
|
cfg.rcRate8 = 90;
|
|
cfg.rcExpo8 = 65;
|
|
cfg.rollPitchRate = 0;
|
|
cfg.yawRate = 0;
|
|
cfg.dynThrPID = 0;
|
|
cfg.thrMid8 = 50;
|
|
cfg.thrExpo8 = 0;
|
|
// for (i = 0; i < CHECKBOXITEMS; i++)
|
|
// cfg.activate[i] = 0;
|
|
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.accz_deadband = 40;
|
|
cfg.accxy_deadband = 40;
|
|
cfg.baro_tab_size = 21;
|
|
cfg.baro_noise_lpf = 0.6f;
|
|
cfg.baro_cf_vel = 0.985f;
|
|
cfg.baro_cf_alt = 0.965f;
|
|
cfg.acc_unarmedcal = 1;
|
|
|
|
// Radio
|
|
parseRcChannels("AETR1234");
|
|
cfg.deadband = 0;
|
|
cfg.yawdeadband = 0;
|
|
cfg.alt_hold_throttle_neutral = 40;
|
|
cfg.alt_hold_fast_change = 1;
|
|
cfg.throttle_angle_correction = 0; // could be 40
|
|
|
|
// 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.
|
|
cfg.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
|
|
|
|
// servos
|
|
for (i = 0; i < 8; i++) {
|
|
cfg.servoConf[i].min = 1020;
|
|
cfg.servoConf[i].max = 2000;
|
|
cfg.servoConf[i].middle = 1500;
|
|
cfg.servoConf[i].rate = servoRates[i];
|
|
}
|
|
|
|
cfg.yaw_direction = 1;
|
|
cfg.tri_unarmed_servo = 1;
|
|
|
|
// gimbal
|
|
cfg.gimbal_flags = GIMBAL_NORMAL;
|
|
|
|
// gps/nav stuff
|
|
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;
|
|
cfg.ap_mode = 40;
|
|
|
|
// custom mixer. clear by defaults.
|
|
for (i = 0; i < MAX_MOTORS; i++)
|
|
mcfg.customMixer[i].throttle = 0.0f;
|
|
|
|
// 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)
|
|
{
|
|
return enabledSensors & mask;
|
|
}
|
|
|
|
void sensorsSet(uint32_t mask)
|
|
{
|
|
enabledSensors |= mask;
|
|
}
|
|
|
|
void sensorsClear(uint32_t mask)
|
|
{
|
|
enabledSensors &= ~(mask);
|
|
}
|
|
|
|
uint32_t sensorsMask(void)
|
|
{
|
|
return enabledSensors;
|
|
}
|
|
|
|
bool feature(uint32_t mask)
|
|
{
|
|
return mcfg.enabledFeatures & mask;
|
|
}
|
|
|
|
void featureSet(uint32_t mask)
|
|
{
|
|
mcfg.enabledFeatures |= mask;
|
|
}
|
|
|
|
void featureClear(uint32_t mask)
|
|
{
|
|
mcfg.enabledFeatures &= ~(mask);
|
|
}
|
|
|
|
void featureClearAll()
|
|
{
|
|
mcfg.enabledFeatures = 0;
|
|
}
|
|
|
|
uint32_t featureMask(void)
|
|
{
|
|
return mcfg.enabledFeatures;
|
|
}
|