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

merged multiwii GPS code from 1097. still no support for UBX, or GPS auto-config, soon.

Browse source 
added interrupt pins from mag/mma/mpu for rev4 hardware. nothing done with them yet - candidates for EXTI use
added tx buffer to UART2 (gps) in preparation for auto-config


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@203 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2012-09-04 09:13:59 +00:00
parent 70db9006af
commit 80d7ba604b
12 changed files with 2769 additions and 2694 deletions
Download patch file Download diff file Expand all files Collapse all files

5129
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

1
src/cli.c
View file

@ -129,6 +129,7 @@ const clivalue_t valueTable[] = {
{ "gyro_cmpf_factor", VAR_UINT16, &cfg.gyro_cmpf_factor, 100, 1000 },
{ "mpu6050_scale", VAR_UINT8, &cfg.mpu6050_scale, 0, 1 },
{ "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 },

5
src/config.c
View file

@ -13,7 +13,7 @@ config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 26;
uint8_t checkNewConf = 27;
void parseRcChannels(const char *input)
{
@ -184,6 +184,7 @@ void checkFirstTime(bool reset)
cfg.gimbal_roll_mid = 1500;
// gps/nav stuff
cfg.gps_type = 0; // NMEA
cfg.gps_baudrate = 9600;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
@ -192,7 +193,7 @@ void checkFirstTime(bool reset)
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;
// serial(uart1) baudrate
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
writeParams(0);

9
src/drv_hmc5883l.c
View file

@ -1,6 +1,7 @@
#include "board.h"
// HMC5883L, default address 0x1E
// PB12 connected to MAG_DRDY on rev4 hardware
#define MAG_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03
@ -43,6 +44,14 @@ bool hmc5883lDetect(void)
void hmc5883lInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// PB12 - MAG_DRDY output on rev4 hardware
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
delay(100);
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation);
delay(50);

9
src/drv_mma845x.c
View file

@ -1,6 +1,7 @@
#include "board.h"
// MMA8452QT, Standard address 0x1C
// ACC_INT2 routed to PA5
#define MMA8452_ADDRESS 0x1C
@ -71,6 +72,14 @@ bool mma8452Detect(sensor_t *acc)
static void mma8452Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// PA5 - ACC_INT2 output on rev3/4 hardware
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
i2cWrite(MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G);
i2cWrite(MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4);

10
src/drv_mpu6050.c
View file

@ -1,7 +1,9 @@
#include "board.h"
// MPU6050, Standard address 0x68
// MPU_INT on PB13 on rev4 hardware
#define MPU6050_ADDRESS 0x68
// Experimental DMP support
// #define MPU6050_DMP
@ -210,6 +212,14 @@ static void mpu6050AccAlign(int16_t * accData)
static void mpu6050GyroInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// PB13 - MPU_INT output on rev4 hardware
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
#ifndef MPU6050_DMP
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);

49
src/drv_uart.c
View file

@ -9,7 +9,6 @@
// Receive buffer, circular DMA
volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
uint32_t rxDMAPos = 0;
volatile uint8_t txBuffer[UART_BUFFER_SIZE];
uint32_t txBufferTail = 0;
uint32_t txBufferHead = 0;
@ -44,10 +43,8 @@ void uartInit(uint32_t speed)
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
// UART
// USART1_TX PA9
// USART1_RX PA10
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
@ -153,8 +150,14 @@ void uartPrint(char *str)
/* -------------------------- UART2 (Spektrum, GPS) ----------------------------- */
uartReceiveCallbackPtr uart2Callback = NULL;
#define UART2_BUFFER_SIZE 64
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func)
volatile uint8_t tx2Buffer[UART2_BUFFER_SIZE];
uint32_t tx2BufferTail = 0;
uint32_t tx2BufferHead = 0;
bool uart2RxOnly = false;
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
{
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
@ -162,33 +165,65 @@ void uart2Init(uint32_t speed, uartReceiveCallbackPtr func)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
uart2RxOnly = rxOnly;
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// USART2_TX PA2
// USART2_RX PA3
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
if (!rxOnly)
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOA, &GPIO_InitStructure);
USART_InitStructure.USART_BaudRate = speed;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_Mode = USART_Mode_Rx;
USART_InitStructure.USART_Mode = USART_Mode_Rx | (rxOnly ? 0 : USART_Mode_Tx);
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_Init(USART2, &USART_InitStructure);
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
if (!rxOnly)
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
USART_Cmd(USART2, ENABLE);
uart2Callback = func;
}
void uart2Write(uint8_t ch)
{
if (uart2RxOnly)
return;
tx2Buffer[tx2BufferHead] = ch;
tx2BufferHead = (tx2BufferHead + 1) % UART2_BUFFER_SIZE;
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
}
void USART2_IRQHandler(void)
{
if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) {
uint16_t SR = USART2->SR;
if (SR & USART_IT_RXNE) {
if (uart2Callback)
uart2Callback(USART_ReceiveData(USART2));
}
if (SR & USART_FLAG_TXE) {
if (tx2BufferTail != tx2BufferHead) {
USART2->DR = tx2Buffer[tx2BufferTail];
tx2BufferTail = (tx2BufferTail + 1) % UART2_BUFFER_SIZE;
} else {
USART_ITConfig(USART2, USART_IT_TXE, DISABLE);
}
}
}

6
src/drv_uart.h
View file

@ -1,5 +1,6 @@
#pragma once
// USART1
void uartInit(uint32_t speed);
uint16_t uartAvailable(void);
bool uartTransmitEmpty(void);
@ -7,4 +8,7 @@ uint8_t uartRead(void);
uint8_t uartReadPoll(void);
void uartWrite(uint8_t ch);
void uartPrint(char *str);
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func);
// USART2 (
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly);
void uart2Write(uint8_t ch);

211
src/gps.c
View file

@ -11,7 +11,7 @@ static void GPS_set_pids(void);
void gpsInit(uint32_t baudrate)
{
GPS_set_pids();
uart2Init(baudrate, GPS_NewData);
uart2Init(baudrate, GPS_NewData, false);
// catch some GPS frames. TODO check this
delay(500);
@ -21,7 +21,7 @@ void gpsInit(uint32_t baudrate)
/*-----------------------------------------------------------
*
* Multiwii GPS code - revision: 851
* Multiwii GPS code - revision: 1097
*
*-----------------------------------------------------------*/
#define POSHOLD_IMAX 20 // degrees
@ -45,6 +45,7 @@ static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(int max_speed);
static void GPS_update_crosstrack(void);
static bool GPS_newFrame(char c);
static bool GPS_NMEA_newFrame(char c);
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);
@ -56,91 +57,62 @@ typedef struct {
float Imax;
} PID_PARAM;
static PID_PARAM posholdPID;
static PID_PARAM poshold_ratePID;
static PID_PARAM navPID;
static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
// AC_PID.h & AC_PID.cpp
typedef struct {
float _integrator; ///< integrator value
int32_t _last_input; ///< last input for derivative
float _last_derivative; ///< last derivative for low-pass filter
float _output;
float _derivative;
} AC_PID;
float integrator; // integrator value
int32_t last_input; // last input for derivative
float last_derivative; // last derivative for low-pass filter
float output;
float derivative;
} PID;
static float AC_PID_get_integrator(AC_PID * ac_pid)
static PID posholdPID[2];
static PID poshold_ratePID[2];
static PID navPID[2];
static int32_t get_P(int32_t error, PID_PARAM *pid)
{
return ac_pid->_integrator;
return (float)error * pid->kP;
}
static void AC_PID_set_integrator(AC_PID * ac_pid, float i)
static int32_t get_I(int32_t error, float *dt, PID *pid, PID_PARAM *pid_param)
{
ac_pid->_integrator = i;
pid->integrator += ((float)error * pid_param->kI) * *dt;
pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax);
return pid->integrator;
}
/// Low pass filter cut frequency for derivative calculation.
// static const float ac_pid_filter = 1.0f / (2.0f * M_PI * (float)cfg.gps_lpf); // Set to "1 / ( 2 * PI * f_cut )"
#define AC_PID_FILTER (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf))
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
static int32_t AC_PID_get_p(AC_PID * ac_pid, int32_t error, PID_PARAM * pid)
static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
{
return (float) error *pid->kP;
}
pid->derivative = (input - pid->last_input) / *dt;
static int32_t AC_PID_get_i(AC_PID * ac_pid, int32_t error, float *dt, PID_PARAM * pid)
{
ac_pid->_integrator += ((float) error * pid->kI) * *dt;
if (ac_pid->_integrator < -pid->Imax) {
ac_pid->_integrator = -pid->Imax;
} else if (ac_pid->_integrator > pid->Imax) {
ac_pid->_integrator = pid->Imax;
}
return ac_pid->_integrator;
}
static int32_t AC_PID_get_d(AC_PID * ac_pid, int32_t input, float *dt, PID_PARAM * pid)
{
ac_pid->_derivative = (input - ac_pid->_last_input) / *dt;
// Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf )"
#define PID_FILTER (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf))
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
ac_pid->_derivative = ac_pid->_last_derivative + (*dt / (AC_PID_FILTER + *dt)) * (ac_pid->_derivative - ac_pid->_last_derivative);
pid->derivative = pid->last_derivative + (*dt / (PID_FILTER + *dt)) * (pid->derivative - pid->last_derivative);
// update state
ac_pid->_last_input = input;
ac_pid->_last_derivative = ac_pid->_derivative;
pid->last_input = input;
pid->last_derivative = pid->derivative;
// add in derivative component
return pid->kD * ac_pid->_derivative;
return pid_param->kD * pid->derivative;
}
/// Reset the PID integrator
///
static void AC_PID_reset(AC_PID * ac_pid)
static void reset_PID(PID *pid)
{
ac_pid->_integrator = 0;
ac_pid->_last_input = 0;
ac_pid->_last_derivative = 0;
pid->integrator = 0;
pid->last_input = 0;
pid->last_derivative = 0;
}
#define _X 1
#define _Y 0
/****************** PI and PID controllers for GPS ********************///32938 -> 33160
static AC_PID pi_poshold[2];
static AC_PID pid_poshold_rate[2];
static AC_PID pid_nav[2];
#define RADX100 0.000174532925f
#define CROSSTRACK_GAIN 1
@ -216,19 +188,13 @@ void GPS_NewData(uint16_t c)
else
GPS_update = 1;
if (f.GPS_FIX && GPS_numSat >= 5) {
if (!f.ARMED) {
if (!f.ARMED)
f.GPS_FIX_HOME = 0;
}
if (!f.GPS_FIX_HOME && f.ARMED) {
f.GPS_FIX_HOME = 1;
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
nav_takeoff_bearing = heading; // save takeoff heading
}
if (!f.GPS_FIX_HOME && f.ARMED)
GPS_reset_home_position();
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
@ -241,7 +207,7 @@ void GPS_NewData(uint16_t c)
GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode...
if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999)
GPS_coord[axis] = GPS_filtered[axis];
}
@ -259,7 +225,12 @@ void GPS_NewData(uint16_t c)
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
//calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
if (!f.GPS_FIX_HOME) { // If we don't have home set, do not display anything
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE) { // ok we are navigating
@ -274,7 +245,7 @@ void GPS_NewData(uint16_t c)
break;
case NAV_MODE_WP:
speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); //slow navigation
speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); // slow navigation
// use error as the desired rate towards the target
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed);
@ -306,8 +277,9 @@ void GPS_reset_home_position(void)
if (f.GPS_FIX && GPS_numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
nav_takeoff_bearing = heading; //save takeoff heading
//Set ground altitude
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
nav_takeoff_bearing = heading; // save takeoff heading
// Set ground altitude
f.GPS_FIX_HOME = 1;
}
}
@ -319,30 +291,30 @@ void GPS_reset_nav(void)
for (i = 0; i < 2; i++) {
GPS_angle[i] = 0;
nav_rated[i] = 0;
nav[i] = 0;
AC_PID_reset(&pi_poshold[i]);
AC_PID_reset(&pid_poshold_rate[i]);
AC_PID_reset(&pid_nav[i]);
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
reset_PID(&navPID[i]);
}
}
//Get the relevant P I D values and set the PID controllers
static void GPS_set_pids(void)
{
posholdPID.kP = (float) cfg.P8[PIDPOS] / 100.0f;
posholdPID.kI = (float) cfg.I8[PIDPOS] / 100.0f;
posholdPID.Imax = POSHOLD_RATE_IMAX * 100;
posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f;
posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID.kP = (float) cfg.P8[PIDPOSR] / 10.0f;
poshold_ratePID.kI = (float) cfg.I8[PIDPOSR] / 100.0f;
poshold_ratePID.kD = (float) cfg.D8[PIDPOSR] / 1000.0f;
poshold_ratePID.Imax = POSHOLD_RATE_IMAX * 100;
navPID.kP = (float) cfg.P8[PIDNAVR] / 10.0f;
navPID.kI = (float) cfg.I8[PIDNAVR] / 100.0f;
navPID.kD = (float) cfg.D8[PIDNAVR] / 1000.0f;
navPID.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID_PARAM.kP = (float)cfg.P8[PIDPOSR] / 10.0f;
poshold_ratePID_PARAM.kI = (float)cfg.I8[PIDPOSR] / 100.0f;
poshold_ratePID_PARAM.kD = (float)cfg.D8[PIDPOSR] / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
navPID_PARAM.kP = (float)cfg.P8[PIDNAVR] / 10.0f;
navPID_PARAM.kI = (float)cfg.I8[PIDNAVR] / 100.0f;
navPID_PARAM.kD = (float)cfg.D8[PIDNAVR] / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
}
// OK here is the onboard GPS code
@ -359,14 +331,14 @@ static void GPS_set_pids(void)
//
static void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (abs((float)lat)) * (0.0174532925f / 10000000.0f);
float rads = (abs((float)lat) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cosf(rads);
}
////////////////////////////////////////////////////////////////////////////////////
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
//
void GPS_set_next_wp(int32_t * lat, int32_t * lon)
void GPS_set_next_wp(int32_t *lat, int32_t *lon)
{
GPS_WP[LAT] = *lat;
GPS_WP[LON] = *lon;
@ -453,7 +425,7 @@ static void GPS_calc_velocity(void)
// 3000 = 33m
// 10000 = 111m
//
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng)
static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng)
{
error[LON] = (float) (*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
error[LAT] = *target_lat - *gps_lat; // Y Error
@ -464,28 +436,28 @@ static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng,
//
static void GPS_calc_poshold(void)
{
int32_t p, i, d;
int32_t output;
int32_t d;
int32_t target_speed;
int axis;
for (axis = 0; axis < 2; axis++) {
target_speed = AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID); // calculate desired speed from lon error
target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lon error
rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error
p = AC_PID_get_p(&pid_poshold_rate[axis], rate_error[axis], &poshold_ratePID);
i = AC_PID_get_i(&pid_poshold_rate[axis], rate_error[axis] + error[axis], &dTnav, &poshold_ratePID);
d = AC_PID_get_d(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);
nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +
get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
d = constrain(d, -2000, 2000);
// get rid of noise
#if defined(GPS_LOW_SPEED_D_FILTER)
if (abs(actual_speed[axis]) < 50)
d = 0;
#endif
output = p + i + d;
nav[axis] = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX);
AC_PID_set_integrator(&pid_nav[axis], AC_PID_get_integrator(&pid_poshold_rate[axis]));
nav[axis] +=d;
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
navPID[axis].integrator = poshold_ratePID[axis].integrator;
}
}
@ -510,11 +482,12 @@ static void GPS_calc_nav_rate(int max_speed)
rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
// P + I + D
nav[axis] = AC_PID_get_p(&pid_nav[axis], rate_error[axis], &navPID)
+ AC_PID_get_i(&pid_nav[axis], rate_error[axis], &dTnav, &navPID)
+ AC_PID_get_d(&pid_nav[axis], rate_error[axis], &dTnav, &navPID);
nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +
get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +
get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
AC_PID_set_integrator(&pid_poshold_rate[axis], AC_PID_get_integrator(&pid_nav[axis]));
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}
@ -550,7 +523,6 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
// max_speed is default 400 or 4m/s
if (_slow) {
max_speed = min(max_speed, wp_distance / 2);
max_speed = max(max_speed, 0);
} else {
max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, cfg.nav_speed_min); // go at least 100cm/s
@ -694,6 +666,19 @@ static uint8_t hex_c(uint8_t n)
return n;
}
static bool GPS_newFrame(char c)
{
switch (cfg.gps_type) {
case 0: // NMEA
return GPS_NMEA_newFrame(c);
case 1: // UBX
return false; // GPS_UBLOX_newFrame(c);
}
return false;
}
/* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output NMEA frames.
It assumes there are some NMEA GGA frames to decode on the serial bus
@ -709,7 +694,7 @@ static uint8_t hex_c(uint8_t n)
#define FRAME_GGA 1
#define FRAME_RMC 2
static bool GPS_newFrame(char c)
static bool GPS_NMEA_newFrame(char c)
{
uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0;
@ -746,7 +731,9 @@ static bool GPS_newFrame(char c)
}
} else if (frame == FRAME_RMC) {
if (param == 7) {
GPS_speed = ((uint32_t) grab_fields(string, 1) * 5144L) / 1000L; // speed in cm/s added by Mis
GPS_speed = (grab_fields(string, 1) * 5144L) / 1000L; // speed in cm/s added by Mis
} else if (param == 8) {
GPS_ground_course = grab_fields(string, 1); // ground course deg * 10
}
}
param++;

13
src/mw.c
View file

@ -17,7 +17,7 @@ int16_t telemTemperature1; // gyro sensor temperature
int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0;
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
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
@ -487,6 +487,10 @@ void loop(void)
} else {
f.PASSTHRU_MODE = 0;
}
if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.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) {
@ -531,7 +535,6 @@ void loop(void)
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
#ifdef MPU6050_DMP
mpu6050DmpLoop();
#endif
@ -554,7 +557,7 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
@ -564,7 +567,7 @@ void loop(void)
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 ((!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 {
@ -572,7 +575,7 @@ void loop(void)
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]),-cfg.nav_slew_rate, cfg.nav_slew_rate);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {

3
src/mw.h
View file

@ -170,7 +170,8 @@ typedef struct config_t {
uint16_t gimbal_roll_mid; // gimbal roll servo neutral value
// gps-related stuff
uint32_t gps_baudrate;
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

2
src/spektrum.c
View file

@ -27,7 +27,7 @@ void spektrumInit(void)
spek_chan_mask = 0x03;
}
uart2Init(115200, spektrumDataReceive);
uart2Init(115200, spektrumDataReceive, true);
}
// UART2 Receive ISR callback