Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Remove gps_common.c's dependencies on the mw.h/board.h.

Browse source 
Moved some GPS code from mw.c into gps_common.c.
Moved pid values into a pidProfile_t structure; this was done so that
gps_common.c does not have a dependency on config_profile.h.
pidProfile_t lives in flight_common.h now.
Moved gps profile settings from profile_t into gpsProfile_t for the same
reason.
Removed gps_common.c's dependency on masterConfig_t by passing needed
variables into gpsInit().
This commit is contained in:
Dominic Clifton 2014-04-22 00:37:35 +01:00
parent f8d0dd98f7
commit 2c80094b0e
14 changed files with 332 additions and 251 deletions
Download patch file Download diff file Expand all files Collapse all files

85
src/config.c
View file

@ -86,7 +86,8 @@ void activateConfig(void)
generateThrottleCurve(&currentProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle); generateThrottleCurve(&currentProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle);
setPIDController(currentProfile.pidController); setPIDController(currentProfile.pidController);
gpsSetPIDs(); gpsUseProfile(&currentProfile.gpsProfile);
gpsUsePIDs(&currentProfile.pidProfile);
useFailsafeConfig(&currentProfile.failsafeConfig); useFailsafeConfig(&currentProfile.failsafeConfig);
} }
@ -169,6 +170,49 @@ void resetEEPROM(void)
writeEEPROM(); writeEEPROM();
} }
static void resetPidProfile(pidProfile_t *pidProfile)
{
pidProfile->P8[ROLL] = 40;
pidProfile->I8[ROLL] = 30;
pidProfile->D8[ROLL] = 23;
pidProfile->P8[PITCH] = 40;
pidProfile->I8[PITCH] = 30;
pidProfile->D8[PITCH] = 23;
pidProfile->P8[YAW] = 85;
pidProfile->I8[YAW] = 45;
pidProfile->D8[YAW] = 0;
pidProfile->P8[PIDALT] = 50;
pidProfile->I8[PIDALT] = 0;
pidProfile->D8[PIDALT] = 0;
pidProfile->P8[PIDPOS] = 11; // POSHOLD_P * 100;
pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
pidProfile->D8[PIDPOS] = 0;
pidProfile->P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
pidProfile->I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
pidProfile->D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
pidProfile->P8[PIDNAVR] = 14; // NAV_P * 10;
pidProfile->I8[PIDNAVR] = 20; // NAV_I * 100;
pidProfile->D8[PIDNAVR] = 80; // NAV_D * 1000;
pidProfile->P8[PIDLEVEL] = 90;
pidProfile->I8[PIDLEVEL] = 10;
pidProfile->D8[PIDLEVEL] = 100;
pidProfile->P8[PIDMAG] = 40;
pidProfile->P8[PIDVEL] = 120;
pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1;
}
void resetGpsProfile(gpsProfile_t *gpsProfile)
{
gpsProfile->gps_wp_radius = 200;
gpsProfile->gps_lpf = 20;
gpsProfile->nav_slew_rate = 30;
gpsProfile->nav_controls_heading = 1;
gpsProfile->nav_speed_min = 100;
gpsProfile->nav_speed_max = 300;
gpsProfile->ap_mode = 40;
}
// Default settings // Default settings
static void resetConf(void) static void resetConf(void)
{ {
@ -242,34 +286,8 @@ static void resetConf(void)
masterConfig.rssi_aux_channel = 0; masterConfig.rssi_aux_channel = 0;
currentProfile.pidController = 0; currentProfile.pidController = 0;
currentProfile.P8[ROLL] = 40; resetPidProfile(&currentProfile.pidProfile);
currentProfile.I8[ROLL] = 30;
currentProfile.D8[ROLL] = 23;
currentProfile.P8[PITCH] = 40;
currentProfile.I8[PITCH] = 30;
currentProfile.D8[PITCH] = 23;
currentProfile.P8[YAW] = 85;
currentProfile.I8[YAW] = 45;
currentProfile.D8[YAW] = 0;
currentProfile.P8[PIDALT] = 50;
currentProfile.I8[PIDALT] = 0;
currentProfile.D8[PIDALT] = 0;
currentProfile.P8[PIDPOS] = 11; // POSHOLD_P * 100;
currentProfile.I8[PIDPOS] = 0; // POSHOLD_I * 100;
currentProfile.D8[PIDPOS] = 0;
currentProfile.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
currentProfile.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
currentProfile.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
currentProfile.P8[PIDNAVR] = 14; // NAV_P * 10;
currentProfile.I8[PIDNAVR] = 20; // NAV_I * 100;
currentProfile.D8[PIDNAVR] = 80; // NAV_D * 1000;
currentProfile.P8[PIDLEVEL] = 90;
currentProfile.I8[PIDLEVEL] = 10;
currentProfile.D8[PIDLEVEL] = 100;
currentProfile.P8[PIDMAG] = 40;
currentProfile.P8[PIDVEL] = 120;
currentProfile.I8[PIDVEL] = 45;
currentProfile.D8[PIDVEL] = 1;
currentProfile.controlRateConfig.rcRate8 = 90; currentProfile.controlRateConfig.rcRate8 = 90;
currentProfile.controlRateConfig.rcExpo8 = 65; currentProfile.controlRateConfig.rcExpo8 = 65;
currentProfile.controlRateConfig.rollPitchRate = 0; currentProfile.controlRateConfig.rollPitchRate = 0;
@ -322,14 +340,7 @@ static void resetConf(void)
// gimbal // gimbal
currentProfile.gimbal_flags = GIMBAL_NORMAL; currentProfile.gimbal_flags = GIMBAL_NORMAL;
// gps/nav stuff resetGpsProfile(&currentProfile.gpsProfile);
currentProfile.gps_wp_radius = 200;
currentProfile.gps_lpf = 20;
currentProfile.nav_slew_rate = 30;
currentProfile.nav_controls_heading = 1;
currentProfile.nav_speed_min = 100;
currentProfile.nav_speed_max = 300;
currentProfile.ap_mode = 40;
// custom mixer. clear by defaults. // custom mixer. clear by defaults.
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)

14
src/config.h
View file

@ -1,19 +1,5 @@
#pragma once #pragma once
enum {
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
PID_ITEM_COUNT
};
typedef enum { typedef enum {
FEATURE_PPM = 1 << 0, FEATURE_PPM = 1 << 0,
FEATURE_VBAT = 1 << 1, FEATURE_VBAT = 1 << 1,

14
src/config_profile.h
View file

@ -2,9 +2,8 @@
typedef struct profile_s { typedef struct profile_s {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT]; pidProfile_t pidProfile;
uint8_t D8[PID_ITEM_COUNT];
controlRateConfig_t controlRateConfig; controlRateConfig_t controlRateConfig;
@ -46,12 +45,5 @@ typedef struct profile_s {
// gimbal-related configuration // gimbal-related configuration
uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff
// gps-related stuff gpsProfile_t gpsProfile;
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
} profile_t; } profile_t;

2
src/flight_common.c
View file

@ -7,6 +7,8 @@
#include "flight_common.h" #include "flight_common.h"
int16_t heading, magHold;
void mwDisarm(void) void mwDisarm(void)
{ {

23
src/flight_common.h
View file

@ -1,5 +1,26 @@
#pragma once #pragma once
enum {
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
PID_ITEM_COUNT
};
typedef struct pidProfile_s {
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
} pidProfile_t;
enum { enum {
AI_ROLL = 0, AI_ROLL = 0,
AI_PITCH, AI_PITCH,
@ -23,5 +44,7 @@ extern int16_t gyroZero[GYRO_INDEX_COUNT]; // see gyro_index_t
extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT]; extern int32_t accSum[XYZ_AXIS_COUNT];
extern int16_t heading, magHold;
void mwDisarm(void); void mwDisarm(void);

10
src/flight_imu.c
View file

@ -18,6 +18,8 @@
#include "sensors_acceleration.h" #include "sensors_acceleration.h"
#include "sensors_barometer.h" #include "sensors_barometer.h"
#include "gps_common.h"
#include "flight_mixer.h" #include "flight_mixer.h"
#include "boardalignment.h" #include "boardalignment.h"
@ -443,20 +445,20 @@ int getEstimatedAltitude(void)
// Altitude P-Controller // Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500); error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((currentProfile.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s setVel = constrain((currentProfile.pidProfile.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller // Velocity PID-Controller
// P // P
error = setVel - vel_tmp; error = setVel - vel_tmp;
BaroPID = constrain((currentProfile.P8[PIDVEL] * error / 32), -300, +300); BaroPID = constrain((currentProfile.pidProfile.P8[PIDVEL] * error / 32), -300, +300);
// I // I
errorAltitudeI += (currentProfile.I8[PIDVEL] * error) / 8; errorAltitudeI += (currentProfile.pidProfile.I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200)); errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
BaroPID += errorAltitudeI / 1024; // I in range +/-200 BaroPID += errorAltitudeI / 1024; // I in range +/-200
// D // D
BaroPID -= constrain(currentProfile.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); BaroPID -= constrain(currentProfile.pidProfile.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
accZ_old = accZ_tmp; accZ_old = accZ_tmp;
return 1; return 1;

169
src/gps_common.c
View file

@ -1,21 +1,65 @@
#include "board.h" #include <stdbool.h>
#include "mw.h" #include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "common/maths.h" #include "common/maths.h"
#include "flight_common.h" #include "drivers/system_common.h"
#include "drivers/serial_common.h" #include "drivers/serial_common.h"
#include "drivers/serial_uart.h"
#include "serial_common.h" #include "serial_common.h"
#include "drivers/gpio_common.h"
#include "drivers/light_led.h"
#include "common/axis.h"
#include "flight_common.h"
#include "sensors_common.h"
#include "runtime_config.h"
#include "gps_common.h" #include "gps_common.h"
// **********************
// GPS
// **********************
int32_t GPS_coord[2]; // LAT/LON
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t GPS_speed; // speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees * 10
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
static uint8_t gpsProvider;
// GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second) // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500) #define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below // How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (GPS_BAUD_MAX + 1) #define GPS_INIT_ENTRIES (GPS_BAUD_MAX + 1)
#define GPS_BAUD_DELAY (100) #define GPS_BAUD_DELAY (100)
gpsProfile_t *gpsProfile;
typedef struct gpsInitData_t { typedef struct gpsInitData_t {
uint8_t index; uint8_t index;
uint32_t baudrate; uint32_t baudrate;
@ -70,6 +114,8 @@ typedef struct gpsData_t {
gpsData_t gpsData; gpsData_t gpsData;
bool areSticksInApModePosition(uint16_t ap_mode); // FIXME should probably live in rc_sticks.h
static void gpsNewData(uint16_t c); static void gpsNewData(uint16_t c);
static bool gpsNewFrameNMEA(char c); static bool gpsNewFrameNMEA(char c);
static bool gpsNewFrameUBLOX(uint8_t data); static bool gpsNewFrameUBLOX(uint8_t data);
@ -81,9 +127,17 @@ static void gpsSetState(uint8_t state)
gpsData.state_ts = millis(); gpsData.state_ts = millis();
} }
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit() void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
void gpsInit(uint8_t baudrateIndex)
{ {
gpsProfile = gpsProfileToUse;
}
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
{
gpsProvider = initialGpsProvider;
gpsUseProfile(initialGpsProfile);
portMode_t mode = MODE_RXTX; portMode_t mode = MODE_RXTX;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else // init gpsData structure. if we're not actually enabled, don't bother doing anything else
@ -93,10 +147,10 @@ void gpsInit(uint8_t baudrateIndex)
gpsData.lastMessage = millis(); gpsData.lastMessage = millis();
gpsData.errors = 0; gpsData.errors = 0;
// only RX is needed for NMEA-style GPS // only RX is needed for NMEA-style GPS
if (masterConfig.gps_type == GPS_NMEA) if (gpsProvider == GPS_NMEA)
mode = MODE_RX; mode = MODE_RX;
gpsSetPIDs(); gpsUsePIDs(pidProfile);
// Open GPS UART, no callback - buffer will be read out in gpsThread() // Open GPS UART, no callback - buffer will be read out in gpsThread()
serialPorts.gpsport = uartOpen(USART2, NULL, gpsInitData[baudrateIndex].baudrate, mode); serialPorts.gpsport = uartOpen(USART2, NULL, gpsInitData[baudrateIndex].baudrate, mode);
// signal GPS "thread" to initialize when it gets to it // signal GPS "thread" to initialize when it gets to it
@ -105,7 +159,7 @@ void gpsInit(uint8_t baudrateIndex)
void gpsInitHardware(void) void gpsInitHardware(void)
{ {
switch (masterConfig.gps_type) { switch (gpsProvider) {
case GPS_NMEA: case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses // nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate); serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
@ -203,7 +257,7 @@ void gpsThread(void)
static bool gpsNewFrame(uint8_t c) static bool gpsNewFrame(uint8_t c)
{ {
switch (masterConfig.gps_type) { switch (gpsProvider) {
case GPS_NMEA: // NMEA case GPS_NMEA: // NMEA
case GPS_MTK_NMEA: // MTK in NMEA mode case GPS_MTK_NMEA: // MTK in NMEA mode
return gpsNewFrameNMEA(c); return gpsNewFrameNMEA(c);
@ -313,7 +367,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// Low pass filter cut frequency for derivative calculation // Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf ) // Set to "1 / ( 2 * PI * gps_lpf )
float pidFilter = (1.0f / (2.0f * M_PI * (float)currentProfile.gps_lpf)); float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf));
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative); pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
@ -471,13 +525,13 @@ static void gpsNewData(uint16_t c)
break; break;
case NAV_MODE_WP: case NAV_MODE_WP:
speed = GPS_calc_desired_speed(currentProfile.nav_speed_max, NAV_SLOW_NAV); // slow navigation speed = GPS_calc_desired_speed(gpsProfile->nav_speed_max, NAV_SLOW_NAV); // slow navigation
// use error as the desired rate towards the target // use error as the desired rate towards the target
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100 // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed); GPS_calc_nav_rate(speed);
// Tail control // Tail control
if (currentProfile.nav_controls_heading) { if (gpsProfile->nav_controls_heading) {
if (NAV_TAIL_FIRST) { if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing - 18000) / 100; magHold = wrap_18000(nav_bearing - 18000) / 100;
} else { } else {
@ -485,7 +539,7 @@ static void gpsNewData(uint16_t c)
} }
} }
// Are we there yet ?(within x meters of the destination) // Are we there yet ?(within x meters of the destination)
if ((wp_distance <= currentProfile.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD; nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) { if (NAV_SET_TAKEOFF_HEADING) {
magHold = nav_takeoff_bearing; magHold = nav_takeoff_bearing;
@ -526,20 +580,20 @@ void GPS_reset_nav(void)
} }
// Get the relevant P I D values and set the PID controllers // Get the relevant P I D values and set the PID controllers
void gpsSetPIDs(void) void gpsUsePIDs(pidProfile_t *pidProfile)
{ {
posholdPID_PARAM.kP = (float)currentProfile.P8[PIDPOS] / 100.0f; posholdPID_PARAM.kP = (float)pidProfile->P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)currentProfile.I8[PIDPOS] / 100.0f; posholdPID_PARAM.kI = (float)pidProfile->I8[PIDPOS] / 100.0f;
posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID_PARAM.kP = (float)currentProfile.P8[PIDPOSR] / 10.0f; poshold_ratePID_PARAM.kP = (float)pidProfile->P8[PIDPOSR] / 10.0f;
poshold_ratePID_PARAM.kI = (float)currentProfile.I8[PIDPOSR] / 100.0f; poshold_ratePID_PARAM.kI = (float)pidProfile->I8[PIDPOSR] / 100.0f;
poshold_ratePID_PARAM.kD = (float)currentProfile.D8[PIDPOSR] / 1000.0f; poshold_ratePID_PARAM.kD = (float)pidProfile->D8[PIDPOSR] / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
navPID_PARAM.kP = (float)currentProfile.P8[PIDNAVR] / 10.0f; navPID_PARAM.kP = (float)pidProfile->P8[PIDNAVR] / 10.0f;
navPID_PARAM.kI = (float)currentProfile.I8[PIDNAVR] / 100.0f; navPID_PARAM.kI = (float)pidProfile->I8[PIDNAVR] / 100.0f;
navPID_PARAM.kD = (float)currentProfile.D8[PIDNAVR] / 1000.0f; navPID_PARAM.kD = (float)pidProfile->D8[PIDNAVR] / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
} }
@ -597,7 +651,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
nav_bearing = target_bearing; nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
original_target_bearing = target_bearing; original_target_bearing = target_bearing;
waypoint_speed_gov = currentProfile.nav_speed_min; waypoint_speed_gov = gpsProfile->nav_speed_min;
} }
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
@ -773,7 +827,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
max_speed = min(max_speed, wp_distance / 2); max_speed = min(max_speed, wp_distance / 2);
} else { } else {
max_speed = min(max_speed, wp_distance); max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, currentProfile.nav_speed_min); // go at least 100cm/s max_speed = max(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s
} }
// limit the ramp up of the speed // limit the ramp up of the speed
@ -1269,17 +1323,68 @@ static bool UBLOX_parse_gps(void)
return false; return false;
} }
void updateGpsState(void) void updateGpsStateForHomeAndHoldMode(void)
{ {
float sin_yaw_y = sinf(heading * 0.0174532925f); float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f);
if (currentProfile.nav_slew_rate) { if (gpsProfile->nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -currentProfile.nav_slew_rate, currentProfile.nav_slew_rate); // TODO check this on uint8 nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -currentProfile.nav_slew_rate, currentProfile.nav_slew_rate); nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10; GPS_angle[AI_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; GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else { } else {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10; GPS_angle[AI_ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10; GPS_angle[AI_PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
void updateGpsWaypointsAndMode(void)
{
static uint8_t GPSNavReset = 1;
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] && areSticksInApModePosition(gpsProfile->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_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
nav_mode = NAV_MODE_NONE;
}
}
void updateGpsIndicator(uint32_t currentTime)
{
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
} }
} }

49
src/gps_common.h
View file

@ -21,12 +21,53 @@ typedef enum {
GPS_BAUD_MAX = GPS_BAUD_9600 GPS_BAUD_MAX = GPS_BAUD_9600
} GPSBaudRates; } GPSBaudRates;
// gps // Serial GPS only variables
void gpsInit(uint8_t baudrate); // navigation mode
typedef enum NavigationMode
{
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} NavigationMode;
typedef struct gpsProfile_s {
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
} gpsProfile_t;
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern int32_t GPS_coord[2]; // LAT/LON
extern int32_t GPS_home[2];
extern int32_t GPS_hold[2];
extern uint8_t GPS_numSat;
extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern uint16_t GPS_altitude; // altitude in 0.1m
extern uint16_t GPS_speed; // speed in 0.1m/s
extern uint16_t GPS_ground_course; // degrees * 10
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern int8_t nav_mode; // Navigation mode
void gpsThread(void); void gpsThread(void);
int8_t gpsSetPassthrough(void); int8_t gpsSetPassthrough(void);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_reset_nav(void); void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon); void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void gpsSetPIDs(void); void gpsUseProfile(gpsProfile_t *gpsProfileToUse);
void updateGpsState(void); void gpsUsePIDs(pidProfile_t *pidProfile);
void updateGpsStateForHomeAndHoldMode(void);
void updateGpsWaypointsAndMode(void);
void updateGpsIndicator(uint32_t currentTime);

8
src/main.c
View file

@ -26,6 +26,7 @@ failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe); void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe); void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void buzzerInit(failsafe_t *initialFailsafe); void buzzerInit(failsafe_t *initialFailsafe);
void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
int main(void) int main(void)
{ {
@ -97,7 +98,12 @@ int main(void)
rxInit(&masterConfig.rxConfig, failsafe); rxInit(&masterConfig.rxConfig, failsafe);
if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) { if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) {
gpsInit(masterConfig.gps_baudrate); gpsInit(
masterConfig.gps_baudrate,
masterConfig.gps_type,
&currentProfile.gpsProfile,
&currentProfile.pidProfile
);
} }
#ifdef SONAR #ifdef SONAR

100
src/mw.c
View file

@ -39,28 +39,6 @@ int16_t axisPID[3];
extern failsafe_t *failsafe; extern failsafe_t *failsafe;
// **********************
// GPS
// **********************
int32_t GPS_coord[2];
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees * 10
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
// Automatic ACC Offset Calibration // Automatic ACC Offset Calibration
bool AccInflightCalibrationArmed = false; bool AccInflightCalibrationArmed = false;
bool AccInflightCalibrationMeasurementDone = false; bool AccInflightCalibrationMeasurementDone = false;
@ -68,6 +46,11 @@ bool AccInflightCalibrationSavetoEEProm = false;
bool AccInflightCalibrationActive = false; bool AccInflightCalibrationActive = false;
uint16_t InflightcalibratingA = 0; uint16_t InflightcalibratingA = 0;
bool areSticksInApModePosition(uint16_t ap_mode) // FIXME should probably live in rc_sticks.h
{
return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
}
void annexCode(void) void annexCode(void)
{ {
static uint32_t calibratedAccTime; static uint32_t calibratedAccTime;
@ -115,9 +98,9 @@ void annexCode(void)
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
} }
dynP8[axis] = (uint16_t)currentProfile.P8[axis] * prop1 / 100; dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile.I8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile.D8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100;
if (rcData[axis] < masterConfig.rxConfig.midrc) if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis]; rcCommand[axis] = -rcCommand[axis];
} }
@ -184,11 +167,7 @@ void annexCode(void)
} }
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime; updateGpsIndicator(currentTime);
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
} }
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
@ -238,14 +217,14 @@ static void pidMultiWii(void)
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination // 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis]; errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis];
PTermACC = errorAngle * currentProfile.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = errorAngle * currentProfile.pidProfile.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -currentProfile.D8[PIDLEVEL] * 5, +currentProfile.D8[PIDLEVEL] * 5); PTermACC = constrain(PTermACC, -currentProfile.pidProfile.D8[PIDLEVEL] * 5, +currentProfile.pidProfile.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * currentProfile.I8[PIDLEVEL]) >> 12; ITermACC = (errorAngleI[axis] * currentProfile.pidProfile.I8[PIDLEVEL]) >> 12;
} }
if (!f.ANGLE_MODE || f.HORIZON_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 / currentProfile.P8[axis]; error = (int32_t)rcCommand[axis] * 10 * 8 / currentProfile.pidProfile.P8[axis];
error -= gyroData[axis]; error -= gyroData[axis];
PTermGYRO = rcCommand[axis]; PTermGYRO = rcCommand[axis];
@ -253,7 +232,7 @@ static void pidMultiWii(void)
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640) if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0; errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * currentProfile.I8[axis]) >> 6; ITermGYRO = (errorGyroI[axis] / 125 * currentProfile.pidProfile.I8[axis]) >> 6;
} }
if (f.HORIZON_MODE && axis < 2) { if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
@ -305,10 +284,10 @@ static void pidRewrite(void)
AngleRateTmp = ((int32_t) (currentProfile.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4; AngleRateTmp = ((int32_t) (currentProfile.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) { if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel // mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * currentProfile.I8[PIDLEVEL]) >> 8; AngleRateTmp += (errorAngle * currentProfile.pidProfile.I8[PIDLEVEL]) >> 8;
} }
} else { // it's the ANGLE mode - control is angle based, so control loop is needed } else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * currentProfile.P8[PIDLEVEL]) >> 4; AngleRateTmp = (errorAngle * currentProfile.pidProfile.P8[PIDLEVEL]) >> 4;
} }
} }
@ -319,13 +298,13 @@ static void pidRewrite(void)
RateError = AngleRateTmp - gyroData[axis]; RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component // -----calculate P component
PTerm = (RateError * currentProfile.P8[axis]) >> 7; PTerm = (RateError * currentProfile.pidProfile.P8[axis]) >> 7;
// -----calculate I component // -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced. // there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time) // Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048. // is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * currentProfile.I8[axis]; errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * currentProfile.pidProfile.I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings // I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -343,7 +322,7 @@ static void pidRewrite(void)
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
DTerm = (deltaSum * currentProfile.D8[axis]) >> 8; DTerm = (deltaSum * currentProfile.pidProfile.D8[axis]) >> 8;
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;
@ -375,7 +354,6 @@ void loop(void)
#endif #endif
static uint32_t loopTime; static uint32_t loopTime;
uint16_t auxState = 0; uint16_t auxState = 0;
static uint8_t GPSNavReset = 1;
bool isThrottleLow = false; bool isThrottleLow = false;
bool rcReady = false; bool rcReady = false;
@ -640,41 +618,7 @@ void loop(void)
#endif #endif
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if (f.GPS_FIX && GPS_numSat >= 5) { updateGpsWaypointsAndMode();
// 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] && abs(rcCommand[ROLL]) < currentProfile.ap_mode && abs(rcCommand[PITCH]) < currentProfile.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_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
nav_mode = NAV_MODE_NONE;
}
} }
if (rcOptions[BOXPASSTHRU]) { if (rcOptions[BOXPASSTHRU]) {
@ -750,7 +694,7 @@ void loop(void)
dif -= 360; dif -= 360;
dif *= -masterConfig.yaw_control_direction; dif *= -masterConfig.yaw_control_direction;
if (f.SMALL_ANGLE) if (f.SMALL_ANGLE)
rcCommand[YAW] -= dif * currentProfile.P8[PIDMAG] / 30; // 18 deg rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30; // 18 deg
} else } else
magHold = heading; magHold = heading;
} }
@ -808,7 +752,7 @@ void loop(void)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
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) {
updateGpsState(); updateGpsStateForHomeAndHoldMode();
} }
} }

31
src/mw.h
View file

@ -10,15 +10,6 @@
#define VERSION 230 #define VERSION 230
// Serial GPS only variables
// navigation mode
typedef enum NavigationMode
{
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} NavigationMode;
#include "gimbal.h" #include "gimbal.h"
#include "flight_mixer.h" #include "flight_mixer.h"
@ -34,6 +25,7 @@ enum {
#include "serial_common.h" #include "serial_common.h"
#include "rx_common.h" #include "rx_common.h"
#include "gps_common.h"
#include "config.h" #include "config.h"
#include "config_profile.h" #include "config_profile.h"
#include "config_master.h" #include "config_master.h"
@ -64,7 +56,6 @@ extern int32_t BaroPID;
extern int32_t vario; extern int32_t vario;
extern int16_t throttleAngleCorrection; extern int16_t throttleAngleCorrection;
extern int16_t headFreeModeHold; extern int16_t headFreeModeHold;
extern int16_t heading, magHold;
extern int16_t motor[MAX_MOTORS]; extern int16_t motor[MAX_MOTORS];
extern int16_t servo[MAX_SERVOS]; extern int16_t servo[MAX_SERVOS];
extern uint16_t rssi; // range: [0;1023] extern uint16_t rssi; // range: [0;1023]
@ -72,26 +63,6 @@ extern uint8_t vbat;
extern int16_t telemTemperature1; // gyro sensor temperature extern int16_t telemTemperature1; // gyro sensor temperature
extern uint8_t toggleBeep; extern uint8_t toggleBeep;
// GPS stuff
extern int32_t GPS_coord[2];
extern int32_t GPS_home[2];
extern int32_t GPS_hold[2];
extern uint8_t GPS_numSat;
extern uint16_t GPS_distanceToHome; // distance to home or hold point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m and speed in 0.1m/s
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
extern uint16_t GPS_ground_course; // degrees*10
extern 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 uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern flags_t f; extern flags_t f;
// main // main

2
src/sensors_initialisation.c
View file

@ -8,8 +8,6 @@
#include "sensors_common.h" #include "sensors_common.h"
int16_t heading, magHold;
extern uint16_t batteryWarningVoltage; extern uint16_t batteryWarningVoltage;
extern uint8_t batteryCellCount; extern uint8_t batteryCellCount;
extern float magneticDeclination; extern float magneticDeclination;

64
src/serial_cli.c
View file

@ -201,38 +201,38 @@ const clivalue_t valueTable[] = {
{ "baro_cf_vel", VAR_FLOAT, &currentProfile.baro_cf_vel, 0, 1 }, { "baro_cf_vel", VAR_FLOAT, &currentProfile.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT, &currentProfile.baro_cf_alt, 0, 1 }, { "baro_cf_alt", VAR_FLOAT, &currentProfile.baro_cf_alt, 0, 1 },
{ "mag_declination", VAR_INT16, &currentProfile.mag_declination, -18000, 18000 }, { "mag_declination", VAR_INT16, &currentProfile.mag_declination, -18000, 18000 },
{ "gps_pos_p", VAR_UINT8, &currentProfile.P8[PIDPOS], 0, 200 }, { "gps_pos_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &currentProfile.I8[PIDPOS], 0, 200 }, { "gps_pos_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8, &currentProfile.D8[PIDPOS], 0, 200 }, { "gps_pos_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8, &currentProfile.P8[PIDPOSR], 0, 200 }, { "gps_posr_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8, &currentProfile.I8[PIDPOSR], 0, 200 }, { "gps_posr_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8, &currentProfile.D8[PIDPOSR], 0, 200 }, { "gps_posr_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8, &currentProfile.P8[PIDNAVR], 0, 200 }, { "gps_nav_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8, &currentProfile.I8[PIDNAVR], 0, 200 }, { "gps_nav_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8, &currentProfile.D8[PIDNAVR], 0, 200 }, { "gps_nav_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16, &currentProfile.gps_wp_radius, 0, 2000 }, { "gps_wp_radius", VAR_UINT16, &currentProfile.gpsProfile.gps_wp_radius, 0, 2000 },
{ "nav_controls_heading", VAR_UINT8, &currentProfile.nav_controls_heading, 0, 1 }, { "nav_controls_heading", VAR_UINT8, &currentProfile.gpsProfile.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16, &currentProfile.nav_speed_min, 10, 2000 }, { "nav_speed_min", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &currentProfile.nav_speed_max, 10, 2000 }, { "nav_speed_max", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &currentProfile.nav_slew_rate, 0, 100 }, { "nav_slew_rate", VAR_UINT8, &currentProfile.gpsProfile.nav_slew_rate, 0, 100 },
{ "p_pitch", VAR_UINT8, &currentProfile.P8[PITCH], 0, 200 }, { "p_pitch", VAR_UINT8, &currentProfile.pidProfile.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8, &currentProfile.I8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8, &currentProfile.pidProfile.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8, &currentProfile.D8[PITCH], 0, 200 }, { "d_pitch", VAR_UINT8, &currentProfile.pidProfile.D8[PITCH], 0, 200 },
{ "p_roll", VAR_UINT8, &currentProfile.P8[ROLL], 0, 200 }, { "p_roll", VAR_UINT8, &currentProfile.pidProfile.P8[ROLL], 0, 200 },
{ "i_roll", VAR_UINT8, &currentProfile.I8[ROLL], 0, 200 }, { "i_roll", VAR_UINT8, &currentProfile.pidProfile.I8[ROLL], 0, 200 },
{ "d_roll", VAR_UINT8, &currentProfile.D8[ROLL], 0, 200 }, { "d_roll", VAR_UINT8, &currentProfile.pidProfile.D8[ROLL], 0, 200 },
{ "p_yaw", VAR_UINT8, &currentProfile.P8[YAW], 0, 200 }, { "p_yaw", VAR_UINT8, &currentProfile.pidProfile.P8[YAW], 0, 200 },
{ "i_yaw", VAR_UINT8, &currentProfile.I8[YAW], 0, 200 }, { "i_yaw", VAR_UINT8, &currentProfile.pidProfile.I8[YAW], 0, 200 },
{ "d_yaw", VAR_UINT8, &currentProfile.D8[YAW], 0, 200 }, { "d_yaw", VAR_UINT8, &currentProfile.pidProfile.D8[YAW], 0, 200 },
{ "p_alt", VAR_UINT8, &currentProfile.P8[PIDALT], 0, 200 }, { "p_alt", VAR_UINT8, &currentProfile.pidProfile.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8, &currentProfile.I8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8, &currentProfile.pidProfile.I8[PIDALT], 0, 200 },
{ "d_alt", VAR_UINT8, &currentProfile.D8[PIDALT], 0, 200 }, { "d_alt", VAR_UINT8, &currentProfile.pidProfile.D8[PIDALT], 0, 200 },
{ "p_level", VAR_UINT8, &currentProfile.P8[PIDLEVEL], 0, 200 }, { "p_level", VAR_UINT8, &currentProfile.pidProfile.P8[PIDLEVEL], 0, 200 },
{ "i_level", VAR_UINT8, &currentProfile.I8[PIDLEVEL], 0, 200 }, { "i_level", VAR_UINT8, &currentProfile.pidProfile.I8[PIDLEVEL], 0, 200 },
{ "d_level", VAR_UINT8, &currentProfile.D8[PIDLEVEL], 0, 200 }, { "d_level", VAR_UINT8, &currentProfile.pidProfile.D8[PIDLEVEL], 0, 200 },
{ "p_vel", VAR_UINT8, &currentProfile.P8[PIDVEL], 0, 200 }, { "p_vel", VAR_UINT8, &currentProfile.pidProfile.P8[PIDVEL], 0, 200 },
{ "i_vel", VAR_UINT8, &currentProfile.I8[PIDVEL], 0, 200 }, { "i_vel", VAR_UINT8, &currentProfile.pidProfile.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8, &currentProfile.D8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8, &currentProfile.pidProfile.D8[PIDVEL], 0, 200 },
}; };
#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))

12
src/serial_msp.c
View file

@ -310,9 +310,9 @@ static void evaluateCommand(void)
break; break;
case MSP_SET_PID: case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) { for (i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile.P8[i] = read8(); currentProfile.pidProfile.P8[i] = read8();
currentProfile.I8[i] = read8(); currentProfile.pidProfile.I8[i] = read8();
currentProfile.D8[i] = read8(); currentProfile.pidProfile.D8[i] = read8();
} }
headSerialReply(0); headSerialReply(0);
break; break;
@ -514,9 +514,9 @@ static void evaluateCommand(void)
case MSP_PID: case MSP_PID:
headSerialReply(3 * PID_ITEM_COUNT); headSerialReply(3 * PID_ITEM_COUNT);
for (i = 0; i < PID_ITEM_COUNT; i++) { for (i = 0; i < PID_ITEM_COUNT; i++) {
serialize8(currentProfile.P8[i]); serialize8(currentProfile.pidProfile.P8[i]);
serialize8(currentProfile.I8[i]); serialize8(currentProfile.pidProfile.I8[i]);
serialize8(currentProfile.D8[i]); serialize8(currentProfile.pidProfile.D8[i]);
} }
break; break;
case MSP_PIDNAMES: case MSP_PIDNAMES: