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

Split navigation functionality from io/gps.c into flight/navigation.c.

Browse source 
gps.c now only has code that deals with gps hardware, state and
messaging.
navigation.c  now only has code dealing with flight
navigation/waypoints/home/hold/etc
This commit is contained in:
Dominic Clifton 2014-08-07 14:23:05 +01:00
parent fe9df45a24
commit 9906294cd8
14 changed files with 6131 additions and 6014 deletions
Download patch file Download diff file Expand all files Collapse all files

3
Makefile
View file

@ -156,8 +156,9 @@ COMMON_SRC = build_config.c \
$(DEVICE_STDPERIPH_SRC)
HIGHEND_SRC = flight/autotune.c \
flight/navigation.c \
flight/gps_conversion.c \
io/gps.c \
io/gps_conversion.c \
io/ledstrip.c \
rx/sbus.c \
rx/sumd.c \

1329
src/main/config/config.c
View file

File diff suppressed because it is too large Load diff

261
src/main/flight/altitudehold.c
View file

@ -1,130 +1,131 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/maths.h"
#include "common/axis.h"
#include "drivers/accgyro.h"
#include "drivers/serial.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
// FIXME remove dependency on currentProfile and masterConfig globals and clean up include file list.
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "io/escservo.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/serial.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "telemetry/telemetry.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#ifdef BARO
static int16_t initialThrottleHold;
static void multirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (currentProfile.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
}
}
static void fixedWingAltHold()
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
}
void updateAltHold(void)
{
if (f.FIXED_WING) {
fixedWingAltHold();
} else {
multirotorAltHold();
}
}
void updateAltHoldState(void)
{
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorVelocityI = 0;
BaroPID = 0;
}
} else {
f.BARO_MODE = 0;
}
}
#endif
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/maths.h"
#include "common/axis.h"
#include "drivers/accgyro.h"
#include "drivers/serial.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
// FIXME remove dependency on currentProfile and masterConfig globals and clean up include file list.
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "io/escservo.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/serial.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "telemetry/telemetry.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#ifdef BARO
static int16_t initialThrottleHold;
static void multirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (currentProfile.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
}
}
static void fixedWingAltHold()
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
}
void updateAltHold(void)
{
if (f.FIXED_WING) {
fixedWingAltHold();
} else {
multirotorAltHold();
}
}
void updateAltHoldState(void)
{
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorVelocityI = 0;
BaroPID = 0;
}
} else {
f.BARO_MODE = 0;
}
}
#endif

136
src/main/io/gps_conversion.c → src/main/flight/gps_conversion.c
View file

@ -1,68 +1,68 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#ifdef GPS
#define DIGIT_TO_VAL(_x) (_x - '0')
uint32_t GPS_coord_to_degrees(char* coordinateString)
{
char *fieldSeparator, *remainingString;
uint8_t degress = 0, minutes = 0;
uint16_t fractionalMinutes = 0;
uint8_t digitIndex;
// scan for decimal point or end of field
for (fieldSeparator = coordinateString; isdigit((unsigned char)*fieldSeparator); fieldSeparator++) {
if (fieldSeparator >= coordinateString + 15)
return 0; // stop potential fail
}
remainingString = coordinateString;
// convert degrees
while ((fieldSeparator - remainingString) > 2) {
if (degress)
degress *= 10;
degress += DIGIT_TO_VAL(*remainingString++);
}
// convert minutes
while (fieldSeparator > remainingString) {
if (minutes)
minutes *= 10;
minutes += DIGIT_TO_VAL(*remainingString++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*fieldSeparator == '.') {
remainingString = fieldSeparator + 1;
for (digitIndex = 0; digitIndex < 4; digitIndex++) {
fractionalMinutes *= 10;
if (isdigit((unsigned char)*remainingString))
fractionalMinutes += *remainingString++ - '0';
}
}
return degress * 10000000UL + (minutes * 1000000UL + fractionalMinutes * 100UL) / 6;
}
#endif
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#ifdef GPS
#define DIGIT_TO_VAL(_x) (_x - '0')
uint32_t GPS_coord_to_degrees(char* coordinateString)
{
char *fieldSeparator, *remainingString;
uint8_t degress = 0, minutes = 0;
uint16_t fractionalMinutes = 0;
uint8_t digitIndex;
// scan for decimal point or end of field
for (fieldSeparator = coordinateString; isdigit((unsigned char)*fieldSeparator); fieldSeparator++) {
if (fieldSeparator >= coordinateString + 15)
return 0; // stop potential fail
}
remainingString = coordinateString;
// convert degrees
while ((fieldSeparator - remainingString) > 2) {
if (degress)
degress *= 10;
degress += DIGIT_TO_VAL(*remainingString++);
}
// convert minutes
while (fieldSeparator > remainingString) {
if (minutes)
minutes *= 10;
minutes += DIGIT_TO_VAL(*remainingString++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*fieldSeparator == '.') {
remainingString = fieldSeparator + 1;
for (digitIndex = 0; digitIndex < 4; digitIndex++) {
fractionalMinutes *= 10;
if (isdigit((unsigned char)*remainingString))
fractionalMinutes += *remainingString++ - '0';
}
}
return degress * 10000000UL + (minutes * 1000000UL + fractionalMinutes * 100UL) / 6;
}
#endif

36
src/main/io/gps_conversion.h → src/main/flight/gps_conversion.h
View file

@ -1,18 +1,18 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
uint32_t GPS_coord_to_degrees(char* s);
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
uint32_t GPS_coord_to_degrees(char* s);

688
src/main/flight/navigation.c Normal file
View file

@ -0,0 +1,688 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "config/config.h"
#include "config/runtime_config.h"
#include "flight/gps_conversion.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/navigation.h"
#ifdef GPS
extern int16_t debug[4];
bool areSticksInApModePosition(uint16_t ap_mode);
// **********************
// GPS
// **********************
int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
static int16_t nav[2];
static int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
static gpsProfile_t *gpsProfile;
void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
{
gpsProfile = gpsProfileToUse;
}
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
{
gpsUseProfile(initialGpsProfile);
gpsUsePIDs(pidProfile);
}
/*-----------------------------------------------------------
*
* Multiwii GPS code - revision: 1097
*
*-----------------------------------------------------------*/
#define POSHOLD_IMAX 20 // degrees
#define POSHOLD_RATE_IMAX 20 // degrees
#define NAV_IMAX 20 // degrees
/* GPS navigation can control the heading */
#define NAV_TAIL_FIRST 0 // true - copter comes in with tail first
#define NAV_SET_TAKEOFF_HEADING 1 // true - when copter arrives to home position it rotates it's head to takeoff direction
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
static bool check_missed_wp(void);
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
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_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
static int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
int16_t last_velocity;
} LeadFilter_PARAM;
typedef struct {
float kP;
float kI;
float kD;
float Imax;
} PID_PARAM;
static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
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;
} 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 (float)error * pid->kP;
}
static int32_t get_I(int32_t error, float *dt, PID *pid, PID_PARAM *pid_param)
{
pid->integrator += ((float)error * pid_param->kI) * *dt;
pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax);
return pid->integrator;
}
static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
{
pid->derivative = (input - pid->last_input) / *dt;
// Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf )
float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf));
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
// update state
pid->last_input = input;
pid->last_derivative = pid->derivative;
// add in derivative component
return pid_param->kD * pid->derivative;
}
static void reset_PID(PID *pid)
{
pid->integrator = 0;
pid->last_input = 0;
pid->last_derivative = 0;
}
#define GPS_X 1
#define GPS_Y 0
/****************** PI and PID controllers for GPS ********************///32938 -> 33160
#define RADX100 0.000174532925f
#define CROSSTRACK_GAIN 1
#define NAV_SLOW_NAV true
#define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];
// Currently used WP
static int32_t GPS_WP[2];
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing;
////////////////////////////////////////////////////////////////////////////////
// Crosstrack
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing;
// distance between plane and home in cm
//static int32_t home_distance;
// distance between plane and next_WP in cm
static uint32_t wp_distance;
// used for slow speed wind up when start navigation;
static int16_t waypoint_speed_gov;
////////////////////////////////////////////////////////////////////////////////////
// moving average filter variables
//
#define GPS_FILTER_VECTOR_LENGTH 5
static uint8_t GPS_filter_index = 0;
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
static int32_t GPS_filter_sum[2];
static int32_t GPS_read[2];
static int32_t GPS_filtered[2];
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)
static uint16_t fraction3[2];
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
static int16_t nav_takeoff_bearing;
void onGpsNewData(void)
{
int axis;
static uint32_t nav_loopTimer;
uint32_t dist;
int32_t dir;
uint16_t speed;
if (!(f.GPS_FIX && GPS_numSat >= 5)) {
return;
}
if (!f.ARMED)
f.GPS_FIX_HOME = 0;
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;
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
// How close we are to a degree line ? its the first three digits from the fractions of degree
// later we use it to Check if we are close to a degree line, if yes, disable averaging,
fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
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 (fraction3[axis] > 1 && fraction3[axis] < 999)
GPS_coord[axis] = GPS_filtered[axis];
}
}
#endif
// dTnav calculation
// Time for calculating x,y speed and navigation pids
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0f);
// calculate distance and bearings for gui and other stuff continously - From home to copter
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
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
// do gps nav calculations here, these are common for nav and poshold
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
switch (nav_mode) {
case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_poshold();
break;
case NAV_MODE_WP:
speed = GPS_calc_desired_speed(gpsProfile->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);
// Tail control
if (gpsProfile->nav_controls_heading) {
if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing - 18000) / 100;
} else {
magHold = nav_bearing / 100;
}
}
// Are we there yet ?(within x meters of the destination)
if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) {
magHold = nav_takeoff_bearing;
}
}
break;
default:
break;
}
} //end of gps calcs
}
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];
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;
}
}
// reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void)
{
int i;
for (i = 0; i < 2; i++) {
GPS_angle[i] = 0;
nav_rated[i] = 0;
nav[i] = 0;
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
void gpsUsePIDs(pidProfile_t *pidProfile)
{
posholdPID_PARAM.kP = (float)pidProfile->P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)pidProfile->I8[PIDPOS] / 100.0f;
posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID_PARAM.kP = (float)pidProfile->P8[PIDPOSR] / 10.0f;
poshold_ratePID_PARAM.kI = (float)pidProfile->I8[PIDPOSR] / 100.0f;
poshold_ratePID_PARAM.kD = (float)pidProfile->D8[PIDPOSR] / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
navPID_PARAM.kP = (float)pidProfile->P8[PIDNAVR] / 10.0f;
navPID_PARAM.kI = (float)pidProfile->I8[PIDNAVR] / 100.0f;
navPID_PARAM.kD = (float)pidProfile->D8[PIDNAVR] / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
}
// OK here is the onboard GPS code
////////////////////////////////////////////////////////////////////////////////////
// PID based GPS navigation functions
// Author : EOSBandi
// Based on code and ideas from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen
// Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
////////////////////////////////////////////////////////////////////////////////////
// this is used to offset the shrinking longitude as we go towards the poles
// It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter
//
static void GPS_calc_longitude_scaling(int32_t lat)
{
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)
{
GPS_WP[LAT] = *lat;
GPS_WP[LON] = *lon;
GPS_calc_longitude_scaling(*lat);
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
original_target_bearing = target_bearing;
waypoint_speed_gov = gpsProfile->nav_speed_min;
}
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
static bool check_missed_wp(void)
{
int32_t temp;
temp = target_bearing - original_target_bearing;
temp = wrap_18000(temp);
return (abs(temp) > 10000); // we passed the waypoint by 100 degrees
}
////////////////////////////////////////////////////////////////////////////////////
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing)
{
float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * 1.113195f;
*bearing = 9000.0f + atan2f(-dLat, dLon) * 5729.57795f; // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
////////////////////////////////////////////////////////////////////////////////////
// keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
//
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing) {
// uint32_t d1;
// int32_t d2;
// GPS_distance_cm_bearing(&lat1,&lon1,&lat2,&lon2,&d1,&d2);
// *dist = d1 / 100; //convert to meters
// *bearing = d2 / 100; //convert to degrees
//}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(void)
{
static int16_t speed_old[2] = { 0, 0 };
static int32_t last[2] = { 0, 0 };
static uint8_t init = 0;
// y_GPS_speed positve = Up
// x_GPS_speed positve = Right
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
speed_old[GPS_X] = actual_speed[GPS_X];
speed_old[GPS_Y] = actual_speed[GPS_Y];
}
init = 1;
last[LON] = GPS_coord[LON];
last[LAT] = GPS_coord[LAT];
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate a location error between two gps coordinates
// Because we are using lat and lon to do our distance errors here's a quick chart:
// 100 = 1m
// 1000 = 11m = 36 feet
// 1800 = 19.80m = 60 feet
// 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)
{
error[LON] = (float)(*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
error[LAT] = *target_lat - *gps_lat; // Y Error
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold(void)
{
int32_t d;
int32_t target_speed;
int axis;
for (axis = 0; axis < 2; axis++) {
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
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
nav[axis] += d;
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
navPID[axis].integrator = poshold_ratePID[axis].integrator;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(uint16_t max_speed)
{
float trig[2];
float temp;
int axis;
// push us towards the original track
GPS_update_crosstrack();
// nav_bearing includes crosstrack
temp = (9000l - nav_bearing) * RADX100;
trig[GPS_X] = cosf(temp);
trig[GPS_Y] = sinf(temp);
for (axis = 0; axis < 2; axis++) {
rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
// P + I + D
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);
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculating cross track error, this tries to keep the copter on a direct line
// when flying to a waypoint.
//
static void GPS_update_crosstrack(void)
{
if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
} else {
nav_bearing = target_bearing;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Determine desired speed when navigating towards a waypoint, also implement slow
// speed rampup when starting a navigation
//
// |< WP Radius
// 0 1 2 3 4 5 6 7 8m
// ...|...|...|...|...|...|...|...|
// 100 | 200 300 400cm/s
// | +|+
// |< we should slow to 1.5 m/s as we hit the target
//
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
{
// max_speed is default 400 or 4m/s
if (_slow) {
max_speed = min(max_speed, wp_distance / 2);
} else {
max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s
}
// limit the ramp up of the speed
// waypoint_speed_gov is reset to 0 at each new WP command
if (max_speed > waypoint_speed_gov) {
waypoint_speed_gov += (int)(100.0f * dTnav); // increase at .5/ms
max_speed = waypoint_speed_gov;
}
return max_speed;
}
////////////////////////////////////////////////////////////////////////////////////
// Utilities
//
static int32_t wrap_18000(int32_t error)
{
if (error > 18000)
error -= 36000;
if (error < -18000)
error += 36000;
return error;
}
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
angle -= 36000;
if (angle < 0)
angle += 36000;
return angle;
}
void updateGpsStateForHomeAndHoldMode(void)
{
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (gpsProfile->nav_slew_rate) {
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]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);
GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[AI_ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 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;
}
}
#endif

55
src/main/flight/navigation.h Normal file
View file

@ -0,0 +1,55 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
// navigation mode
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} navigationMode_e;
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_home[2];
extern int32_t GPS_hold[2];
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 navigationMode_e nav_mode; // Navigation mode
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void gpsUseProfile(gpsProfile_t *gpsProfileToUse);
void gpsUsePIDs(pidProfile_t *pidProfile);
void updateGpsStateForHomeAndHoldMode(void);
void updateGpsWaypointsAndMode(void);
void onGpsNewData(void);

2393
src/main/io/gps.c
View file

File diff suppressed because it is too large Load diff

220
src/main/io/gps.h
View file

@ -1,114 +1,106 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define LAT 0
#define LON 1
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX
} gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_UBLOX
typedef enum {
SBAS_AUTO = 0,
SBAS_EGNOS,
SBAS_WAAS,
SBAS_MSAS,
SBAS_GAGAN
} sbasMode_e;
#define SBAS_MODE_MAX SBAS_GAGAN
typedef enum {
GPS_BAUDRATE_115200 = 0,
GPS_BAUDRATE_57600,
GPS_BAUDRATE_38400,
GPS_BAUDRATE_19200,
GPS_BAUDRATE_9600
} gpsBaudRate_e;
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
// Serial GPS only variables
// navigation mode
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} navigationMode_e;
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;
typedef struct gpsConfig_s {
gpsProvider_e provider;
sbasMode_e sbasMode;
} gpsConfig_t;
typedef enum {
GPS_PASSTHROUGH_ENABLED = 1,
GPS_PASSTHROUGH_NO_GPS,
GPS_PASSTHROUGH_NO_SERIAL_PORT
} gpsEnablePassthroughResult_e;
typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t dddmm;
int16_t mmmm;
} gpsCoordinateDDDMMmmmm_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 uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
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_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 navigationMode_e nav_mode; // Navigation mode
void gpsThread(void);
gpsEnablePassthroughResult_e gpsEnablePassthrough(void);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void gpsUseProfile(gpsProfile_t *gpsProfileToUse);
void gpsUsePIDs(pidProfile_t *pidProfile);
void updateGpsStateForHomeAndHoldMode(void);
void updateGpsWaypointsAndMode(void);
void updateGpsIndicator(uint32_t currentTime);
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define LAT 0
#define LON 1
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX
} gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_UBLOX
typedef enum {
SBAS_AUTO = 0,
SBAS_EGNOS,
SBAS_WAAS,
SBAS_MSAS,
SBAS_GAGAN
} sbasMode_e;
#define SBAS_MODE_MAX SBAS_GAGAN
typedef enum {
GPS_BAUDRATE_115200 = 0,
GPS_BAUDRATE_57600,
GPS_BAUDRATE_38400,
GPS_BAUDRATE_19200,
GPS_BAUDRATE_9600
} gpsBaudRate_e;
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
typedef struct gpsConfig_s {
gpsProvider_e provider;
sbasMode_e sbasMode;
} gpsConfig_t;
typedef enum {
GPS_PASSTHROUGH_ENABLED = 1,
GPS_PASSTHROUGH_NO_GPS,
GPS_PASSTHROUGH_NO_SERIAL_PORT
} gpsEnablePassthroughResult_e;
typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t dddmm;
int16_t mmmm;
} gpsCoordinateDDDMMmmmm_t;
typedef enum {
GPS_MESSAGE_STATE_IDLE = 0,
GPS_MESSAGE_STATE_INIT,
GPS_MESSAGE_STATE_SBAS,
GPS_MESSAGE_STATE_MAX = GPS_MESSAGE_STATE_SBAS
} gpsMessageState_e;
#define GPS_MESSAGE_STATE_ENTRY_COUNT (GPS_MESSAGE_STATE_MAX + 1)
typedef struct gpsData_t {
uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices
uint8_t baudrateIndex; // index into auto-detecting or current baudrate
int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit.
uint32_t lastMessage; // last time valid GPS data was received (millis)
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
uint32_t state_position; // incremental variable for loops
uint32_t state_ts; // timestamp for last state_position increment
gpsMessageState_e messageState;
} gpsData_t;
extern gpsData_t gpsData;
extern int32_t GPS_coord[2]; // LAT/LON
extern uint8_t GPS_numSat;
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
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_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)
void gpsThread(void);
bool gpsNewFrame(uint8_t c);
gpsEnablePassthroughResult_e gpsEnablePassthrough(void);
void updateGpsIndicator(uint32_t currentTime);

2349
src/main/io/serial_cli.c
View file

File diff suppressed because it is too large Load diff

1851
src/main/io/serial_msp.c
View file

File diff suppressed because it is too large Load diff

612
src/main/main.c
View file

@ -1,304 +1,308 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/sound_beeper.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart.h"
#include "drivers/accgyro.h"
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_rx.h"
#include "drivers/adc.h"
#include "flight/flight.h"
#include "flight/mixer.h"
#include "io/serial.h"
#include "flight/failsafe.h"
#include "rx/rx.h"
#include "io/gps.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gimbal.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "telemetry/telemetry.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "build_config.h"
extern rcReadRawDataPtr rcReadRawFunc;
extern uint32_t previousTime;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t *loopbackPort;
#endif
failsafe_t *failsafe;
void initPrintfSupport(void);
void timerInit(void);
void initTelemetry(void);
void serialInit(serialConfig_t *initialSerialConfig);
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void beepcodeInit(failsafe_t *initialFailsafe);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void);
void ledStripInit(void);
void loop(void);
// FIXME bad naming - this appears to be for some new board that hasn't been made available yet.
#ifdef PROD_DEBUG
void productionDebug(void)
{
gpio_config_t gpio;
// remap PB6 to USART1_TX
gpio.pin = Pin_6;
gpio.mode = Mode_AF_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true);
serialInit(mcfg.serial_baudrate);
delay(25);
serialPrint(core.mainport, "DBG ");
printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2);
serialPrint(core.mainport, "EOF");
delay(25);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false);
}
#endif
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
bool sensorsOK = false;
initPrintfSupport();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemInit(masterConfig.emf_avoidance);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adcInit(&adc_params);
initBoardAlignment(&masterConfig.boardAlignment);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile.mag_declination);
// production debug output
#ifdef PROD_DEBUG
productionDebug();
#endif
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3);
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
imuInit();
mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
timerInit();
serialInit(&masterConfig.serialConfig);
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#ifdef STM32F10X_MD
if (!feature(FEATURE_RX_PARALLEL_PWM)) {
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
}
#endif
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
failsafe = failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe);
rxInit(&masterConfig.rxConfig, failsafe);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig,
&currentProfile.gpsProfile,
&currentProfile.pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
Sonar_init();
}
#endif
#ifdef LED_STRIP
if (feature(FEATURE_LED_STRIP)) {
ws2811LedStripInit();
ledStripInit();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY))
initTelemetry();
#endif
previousTime = micros();
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
f.SMALL_ANGLE = 1;
f.PREVENT_ARMING = 0;
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit(&masterConfig.batteryConfig);
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
writeAllMotors(masterConfig.escAndServoConfig.mincommand);
while (1);
}
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/sound_beeper.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart.h"
#include "drivers/accgyro.h"
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_rx.h"
#include "drivers/adc.h"
#include "flight/flight.h"
#include "flight/mixer.h"
#include "io/serial.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "rx/rx.h"
#include "io/gps.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gimbal.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "telemetry/telemetry.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "build_config.h"
extern rcReadRawDataPtr rcReadRawFunc;
extern uint32_t previousTime;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t *loopbackPort;
#endif
failsafe_t *failsafe;
void initPrintfSupport(void);
void timerInit(void);
void initTelemetry(void);
void serialInit(serialConfig_t *initialSerialConfig);
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void beepcodeInit(failsafe_t *initialFailsafe);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void);
void ledStripInit(void);
void loop(void);
// FIXME bad naming - this appears to be for some new board that hasn't been made available yet.
#ifdef PROD_DEBUG
void productionDebug(void)
{
gpio_config_t gpio;
// remap PB6 to USART1_TX
gpio.pin = Pin_6;
gpio.mode = Mode_AF_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true);
serialInit(mcfg.serial_baudrate);
delay(25);
serialPrint(core.mainport, "DBG ");
printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2);
serialPrint(core.mainport, "EOF");
delay(25);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false);
}
#endif
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
bool sensorsOK = false;
initPrintfSupport();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemInit(masterConfig.emf_avoidance);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adcInit(&adc_params);
initBoardAlignment(&masterConfig.boardAlignment);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile.mag_declination);
// production debug output
#ifdef PROD_DEBUG
productionDebug();
#endif
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3);
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
imuInit();
mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
timerInit();
serialInit(&masterConfig.serialConfig);
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#ifdef STM32F10X_MD
if (!feature(FEATURE_RX_PARALLEL_PWM)) {
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
}
#endif
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
failsafe = failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe);
rxInit(&masterConfig.rxConfig, failsafe);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&currentProfile.gpsProfile,
&currentProfile.pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
Sonar_init();
}
#endif
#ifdef LED_STRIP
if (feature(FEATURE_LED_STRIP)) {
ws2811LedStripInit();
ledStripInit();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY))
initTelemetry();
#endif
previousTime = micros();
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
f.SMALL_ANGLE = 1;
f.PREVENT_ARMING = 0;
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit(&masterConfig.batteryConfig);
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
writeAllMotors(masterConfig.escAndServoConfig.mincommand);
while (1);
}

1309
src/main/mw.c
View file

File diff suppressed because it is too large Load diff

903
src/main/telemetry/hott.c
View file

@ -1,451 +1,452 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* telemetry_hott.c
*
* Authors:
* Dominic Clifton - Hydra - Software Serial, Electronics, Hardware Integration and debugging, HoTT Code cleanup and fixes, general telemetry improvements.
* Carsten Giesen - cGiesen - Baseflight port
* Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
* Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed.
*
* https://github.com/obayer/MultiWii-HoTT
* https://github.com/oBayer/MultiHoTT-Module
* https://code.google.com/p/hott-for-ardupilot
*
* HoTT is implemented in Graupner equipment using a bi-directional protocol over a single wire.
*
* Generally the receiver sends a single request byte out using normal uart signals, then waits a short period for a
* multiple byte response and checksum byte before it sends out the next request byte.
* Each response byte must be send with a protocol specific delay between them.
*
* Serial ports use two wires but HoTT uses a single wire so some electronics are required so that
* the signals don't get mixed up. When cleanflight transmits it should not receive it's own transmission.
*
* Connect as follows:
* HoTT TX/RX -> Serial RX (connect directly)
* Serial TX -> 1N4148 Diode -(| )-> HoTT TX/RX (connect via diode)
*
* The diode should be arranged to allow the data signals to flow the right way
* -(| )- == Diode, | indicates cathode marker.
*
* As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins.
*
* Note: The softserial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description
* section. Verify if you require a 5v/3.3v level shifters. The softserial port should not be inverted.
*
* There is a technical discussion (in German) about HoTT here
* http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#ifdef TELEMETRY
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "config/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "flight/flight.h"
#include "io/gps.h"
#include "telemetry/telemetry.h"
#include "telemetry/hott.h"
extern int16_t debug[4];
//#define HOTT_DEBUG
#define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5)
#define HOTT_RX_SCHEDULE 4000
#define HOTT_TX_DELAY_US 3000
static uint32_t lastHoTTRequestCheckAt = 0;
static uint32_t lastMessagesPreparedAt = 0;
static bool hottIsSending = false;
static uint8_t *hottMsg = NULL;
static uint8_t hottMsgRemainingBytesToSendCount;
static uint8_t hottMsgCrc;
#define HOTT_CRC_SIZE (sizeof(hottMsgCrc))
#define HOTT_BAUDRATE 19200
#define HOTT_INITIAL_PORT_MODE MODE_RX
static serialPort_t *hottPort;
static telemetryConfig_t *telemetryConfig;
static HOTT_GPS_MSG_t hottGPSMessage;
static HOTT_EAM_MSG_t hottEAMMessage;
static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
{
memset(msg, 0, size);
msg->start_byte = 0x7C;
msg->eam_sensor_id = HOTT_TELEMETRY_EAM_SENSOR_ID;
msg->sensor_id = HOTT_EAM_SENSOR_TEXT_ID;
msg->stop_byte = 0x7D;
}
#ifdef GPS
typedef enum {
GPS_FIX_CHAR_NONE = '-',
GPS_FIX_CHAR_2D = '2',
GPS_FIX_CHAR_3D = '3',
GPS_FIX_CHAR_DGPS = 'D',
} gpsFixChar_e;
static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
{
memset(msg, 0, size);
msg->start_byte = 0x7C;
msg->gps_sensor_id = HOTT_TELEMETRY_GPS_SENSOR_ID;
msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID;
msg->stop_byte = 0x7D;
}
#endif
static void initialiseMessages(void)
{
initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage));
#ifdef GPS
initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
#endif
}
#ifdef GPS
void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
{
int16_t deg = latitude / 10000000L;
int32_t sec = (latitude - (deg * 10000000L)) * 6;
int8_t min = sec / 1000000L;
sec = (sec % 1000000L) / 100L;
uint16_t degMin = (deg * 100L) + min;
hottGPSMessage->pos_NS = (latitude < 0);
hottGPSMessage->pos_NS_dm_L = degMin;
hottGPSMessage->pos_NS_dm_H = degMin >> 8;
hottGPSMessage->pos_NS_sec_L = sec;
hottGPSMessage->pos_NS_sec_H = sec >> 8;
deg = longitude / 10000000L;
sec = (longitude - (deg * 10000000L)) * 6;
min = sec / 1000000L;
sec = (sec % 1000000L) / 100L;
degMin = (deg * 100L) + min;
hottGPSMessage->pos_EW = (longitude < 0);
hottGPSMessage->pos_EW_dm_L = degMin;
hottGPSMessage->pos_EW_dm_H = degMin >> 8;
hottGPSMessage->pos_EW_sec_L = sec;
hottGPSMessage->pos_EW_sec_H = sec >> 8;
}
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
hottGPSMessage->gps_satelites = GPS_numSat;
if (!f.GPS_FIX) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return;
}
if (GPS_numSat >= 5) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
}
addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);
// GPS Speed in km/h
uint16_t speed = (GPS_speed / 100) * 36; // 0->1m/s * 0->36 = km/h
hottGPSMessage->gps_speed_L = speed & 0x00FF;
hottGPSMessage->gps_speed_H = speed >> 8;
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
uint16_t hottGpsAltitude = (GPS_altitude / 10) + HOTT_GPS_ALTITUDE_OFFSET; // 1 / 0.1f == 10, GPS_altitude of 1 == 0.1m
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
hottGPSMessage->home_direction = GPS_directionToHome;
}
#endif
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
hottEAMMessage->main_voltage_L = vbat & 0xFF;
hottEAMMessage->main_voltage_H = vbat >> 8;
hottEAMMessage->batt1_voltage_L = vbat & 0xFF;
hottEAMMessage->batt1_voltage_H = vbat >> 8;
}
void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
{
// Reset alarms
hottEAMMessage->warning_beeps = 0x0;
hottEAMMessage->alarm_invers1 = 0x0;
hottEAMUpdateBattery(hottEAMMessage);
}
static void hottSerialWrite(uint8_t c)
{
static uint8_t serialWrites = 0;
serialWrites++;
serialWrite(hottPort, c);
}
static portMode_t previousPortMode;
static uint32_t previousBaudRate;
void freeHoTTTelemetryPort(void)
{
// FIXME only need to do this if the port is shared
serialSetMode(hottPort, previousPortMode);
serialSetBaudRate(hottPort, previousBaudRate);
endSerialPortFunction(hottPort, FUNCTION_TELEMETRY);
}
void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
{
telemetryConfig = initialTelemetryConfig;
initialiseMessages();
}
void configureHoTTTelemetryPort(void)
{
hottPort = findOpenSerialPort(FUNCTION_TELEMETRY);
if (hottPort) {
previousPortMode = hottPort->mode;
previousBaudRate = hottPort->baudRate;
//waitForSerialPortToFinishTransmitting(hottPort); // FIXME locks up the system
serialSetBaudRate(hottPort, HOTT_BAUDRATE);
serialSetMode(hottPort, HOTT_INITIAL_PORT_MODE);
beginSerialPortFunction(hottPort, FUNCTION_TELEMETRY);
} else {
hottPort = openSerialPort(FUNCTION_TELEMETRY, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
// FIXME only need to do this if the port is shared
previousPortMode = hottPort->mode;
previousBaudRate = hottPort->baudRate;
}
}
static void hottSendResponse(uint8_t *buffer, int length)
{
if(hottIsSending) {
return;
}
hottMsg = buffer;
hottMsgRemainingBytesToSendCount = length + HOTT_CRC_SIZE;
}
static inline void hottSendGPSResponse(void)
{
hottSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
}
static inline void hottSendEAMResponse(void)
{
hottSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
}
static void hottPrepareMessages(void) {
hottPrepareEAMResponse(&hottEAMMessage);
#ifdef GPS
hottPrepareGPSResponse(&hottGPSMessage);
#endif
}
static void processBinaryModeRequest(uint8_t address) {
#ifdef HOTT_DEBUG
static uint8_t hottBinaryRequests = 0;
static uint8_t hottGPSRequests = 0;
static uint8_t hottEAMRequests = 0;
#endif
switch (address) {
#ifdef GPS
case 0x8A:
#ifdef HOTT_DEBUG
hottGPSRequests++;
#endif
if (sensors(SENSOR_GPS)) {
hottSendGPSResponse();
}
break;
#endif
case 0x8E:
#ifdef HOTT_DEBUG
hottEAMRequests++;
#endif
hottSendEAMResponse();
break;
}
#ifdef HOTT_DEBUG
hottBinaryRequests++;
debug[0] = hottBinaryRequests;
#ifdef GPS
debug[1] = hottGPSRequests;
#endif
debug[2] = hottEAMRequests;
#endif
}
static void flushHottRxBuffer(void)
{
while (serialTotalBytesWaiting(hottPort) > 0) {
serialRead(hottPort);
}
}
static void hottCheckSerialData(uint32_t currentMicros) {
static bool lookingForRequest = true;
uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);
if (bytesWaiting <= 1) {
return;
}
if (bytesWaiting != 2) {
flushHottRxBuffer();
lookingForRequest = true;
return;
}
if (lookingForRequest) {
lastHoTTRequestCheckAt = currentMicros;
lookingForRequest = false;
return;
} else {
bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;
if (!enoughTimePassed) {
return;
}
lookingForRequest = true;
}
uint8_t requestId = serialRead(hottPort);
uint8_t address = serialRead(hottPort);
if (requestId == HOTT_BINARY_MODE_REQUEST_ID) {
processBinaryModeRequest(address);
}
}
static void hottSendTelemetryData(void) {
if (!hottIsSending) {
hottIsSending = true;
serialSetMode(hottPort, MODE_TX);
hottMsgCrc = 0;
return;
}
if (hottMsgRemainingBytesToSendCount == 0) {
hottMsg = NULL;
hottIsSending = false;
serialSetMode(hottPort, MODE_RX);
flushHottRxBuffer();
return;
}
--hottMsgRemainingBytesToSendCount;
if(hottMsgRemainingBytesToSendCount == 0) {
hottSerialWrite(hottMsgCrc++);
return;
}
hottMsgCrc += *hottMsg;
hottSerialWrite(*hottMsg++);
}
static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros)
{
return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ;
}
static inline bool shouldCheckForHoTTRequest()
{
if (hottIsSending) {
return false;
}
return true;
}
void handleHoTTTelemetry(void)
{
static uint32_t serialTimer;
uint32_t now = micros();
if (shouldPrepareHoTTMessages(now)) {
hottPrepareMessages();
lastMessagesPreparedAt = now;
}
if (shouldCheckForHoTTRequest()) {
hottCheckSerialData(now);
}
if (!hottMsg)
return;
if (hottIsSending) {
if(now - serialTimer < HOTT_TX_DELAY_US) {
return;
}
}
hottSendTelemetryData();
serialTimer = now;
}
uint32_t getHoTTTelemetryProviderBaudRate(void) {
return HOTT_BAUDRATE;
}
#endif
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* telemetry_hott.c
*
* Authors:
* Dominic Clifton - Hydra - Software Serial, Electronics, Hardware Integration and debugging, HoTT Code cleanup and fixes, general telemetry improvements.
* Carsten Giesen - cGiesen - Baseflight port
* Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
* Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed.
*
* https://github.com/obayer/MultiWii-HoTT
* https://github.com/oBayer/MultiHoTT-Module
* https://code.google.com/p/hott-for-ardupilot
*
* HoTT is implemented in Graupner equipment using a bi-directional protocol over a single wire.
*
* Generally the receiver sends a single request byte out using normal uart signals, then waits a short period for a
* multiple byte response and checksum byte before it sends out the next request byte.
* Each response byte must be send with a protocol specific delay between them.
*
* Serial ports use two wires but HoTT uses a single wire so some electronics are required so that
* the signals don't get mixed up. When cleanflight transmits it should not receive it's own transmission.
*
* Connect as follows:
* HoTT TX/RX -> Serial RX (connect directly)
* Serial TX -> 1N4148 Diode -(| )-> HoTT TX/RX (connect via diode)
*
* The diode should be arranged to allow the data signals to flow the right way
* -(| )- == Diode, | indicates cathode marker.
*
* As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins.
*
* Note: The softserial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description
* section. Verify if you require a 5v/3.3v level shifters. The softserial port should not be inverted.
*
* There is a technical discussion (in German) about HoTT here
* http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#ifdef TELEMETRY
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "config/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "flight/flight.h"
#include "flight/navigation.h"
#include "io/gps.h"
#include "telemetry/telemetry.h"
#include "telemetry/hott.h"
extern int16_t debug[4];
//#define HOTT_DEBUG
#define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5)
#define HOTT_RX_SCHEDULE 4000
#define HOTT_TX_DELAY_US 3000
static uint32_t lastHoTTRequestCheckAt = 0;
static uint32_t lastMessagesPreparedAt = 0;
static bool hottIsSending = false;
static uint8_t *hottMsg = NULL;
static uint8_t hottMsgRemainingBytesToSendCount;
static uint8_t hottMsgCrc;
#define HOTT_CRC_SIZE (sizeof(hottMsgCrc))
#define HOTT_BAUDRATE 19200
#define HOTT_INITIAL_PORT_MODE MODE_RX
static serialPort_t *hottPort;
static telemetryConfig_t *telemetryConfig;
static HOTT_GPS_MSG_t hottGPSMessage;
static HOTT_EAM_MSG_t hottEAMMessage;
static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
{
memset(msg, 0, size);
msg->start_byte = 0x7C;
msg->eam_sensor_id = HOTT_TELEMETRY_EAM_SENSOR_ID;
msg->sensor_id = HOTT_EAM_SENSOR_TEXT_ID;
msg->stop_byte = 0x7D;
}
#ifdef GPS
typedef enum {
GPS_FIX_CHAR_NONE = '-',
GPS_FIX_CHAR_2D = '2',
GPS_FIX_CHAR_3D = '3',
GPS_FIX_CHAR_DGPS = 'D',
} gpsFixChar_e;
static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
{
memset(msg, 0, size);
msg->start_byte = 0x7C;
msg->gps_sensor_id = HOTT_TELEMETRY_GPS_SENSOR_ID;
msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID;
msg->stop_byte = 0x7D;
}
#endif
static void initialiseMessages(void)
{
initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage));
#ifdef GPS
initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
#endif
}
#ifdef GPS
void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
{
int16_t deg = latitude / 10000000L;
int32_t sec = (latitude - (deg * 10000000L)) * 6;
int8_t min = sec / 1000000L;
sec = (sec % 1000000L) / 100L;
uint16_t degMin = (deg * 100L) + min;
hottGPSMessage->pos_NS = (latitude < 0);
hottGPSMessage->pos_NS_dm_L = degMin;
hottGPSMessage->pos_NS_dm_H = degMin >> 8;
hottGPSMessage->pos_NS_sec_L = sec;
hottGPSMessage->pos_NS_sec_H = sec >> 8;
deg = longitude / 10000000L;
sec = (longitude - (deg * 10000000L)) * 6;
min = sec / 1000000L;
sec = (sec % 1000000L) / 100L;
degMin = (deg * 100L) + min;
hottGPSMessage->pos_EW = (longitude < 0);
hottGPSMessage->pos_EW_dm_L = degMin;
hottGPSMessage->pos_EW_dm_H = degMin >> 8;
hottGPSMessage->pos_EW_sec_L = sec;
hottGPSMessage->pos_EW_sec_H = sec >> 8;
}
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
hottGPSMessage->gps_satelites = GPS_numSat;
if (!f.GPS_FIX) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return;
}
if (GPS_numSat >= 5) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
}
addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);
// GPS Speed in km/h
uint16_t speed = (GPS_speed / 100) * 36; // 0->1m/s * 0->36 = km/h
hottGPSMessage->gps_speed_L = speed & 0x00FF;
hottGPSMessage->gps_speed_H = speed >> 8;
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
uint16_t hottGpsAltitude = (GPS_altitude / 10) + HOTT_GPS_ALTITUDE_OFFSET; // 1 / 0.1f == 10, GPS_altitude of 1 == 0.1m
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
hottGPSMessage->home_direction = GPS_directionToHome;
}
#endif
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
hottEAMMessage->main_voltage_L = vbat & 0xFF;
hottEAMMessage->main_voltage_H = vbat >> 8;
hottEAMMessage->batt1_voltage_L = vbat & 0xFF;
hottEAMMessage->batt1_voltage_H = vbat >> 8;
}
void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
{
// Reset alarms
hottEAMMessage->warning_beeps = 0x0;
hottEAMMessage->alarm_invers1 = 0x0;
hottEAMUpdateBattery(hottEAMMessage);
}
static void hottSerialWrite(uint8_t c)
{
static uint8_t serialWrites = 0;
serialWrites++;
serialWrite(hottPort, c);
}
static portMode_t previousPortMode;
static uint32_t previousBaudRate;
void freeHoTTTelemetryPort(void)
{
// FIXME only need to do this if the port is shared
serialSetMode(hottPort, previousPortMode);
serialSetBaudRate(hottPort, previousBaudRate);
endSerialPortFunction(hottPort, FUNCTION_TELEMETRY);
}
void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
{
telemetryConfig = initialTelemetryConfig;
initialiseMessages();
}
void configureHoTTTelemetryPort(void)
{
hottPort = findOpenSerialPort(FUNCTION_TELEMETRY);
if (hottPort) {
previousPortMode = hottPort->mode;
previousBaudRate = hottPort->baudRate;
//waitForSerialPortToFinishTransmitting(hottPort); // FIXME locks up the system
serialSetBaudRate(hottPort, HOTT_BAUDRATE);
serialSetMode(hottPort, HOTT_INITIAL_PORT_MODE);
beginSerialPortFunction(hottPort, FUNCTION_TELEMETRY);
} else {
hottPort = openSerialPort(FUNCTION_TELEMETRY, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
// FIXME only need to do this if the port is shared
previousPortMode = hottPort->mode;
previousBaudRate = hottPort->baudRate;
}
}
static void hottSendResponse(uint8_t *buffer, int length)
{
if(hottIsSending) {
return;
}
hottMsg = buffer;
hottMsgRemainingBytesToSendCount = length + HOTT_CRC_SIZE;
}
static inline void hottSendGPSResponse(void)
{
hottSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
}
static inline void hottSendEAMResponse(void)
{
hottSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
}
static void hottPrepareMessages(void) {
hottPrepareEAMResponse(&hottEAMMessage);
#ifdef GPS
hottPrepareGPSResponse(&hottGPSMessage);
#endif
}
static void processBinaryModeRequest(uint8_t address) {
#ifdef HOTT_DEBUG
static uint8_t hottBinaryRequests = 0;
static uint8_t hottGPSRequests = 0;
static uint8_t hottEAMRequests = 0;
#endif
switch (address) {
#ifdef GPS
case 0x8A:
#ifdef HOTT_DEBUG
hottGPSRequests++;
#endif
if (sensors(SENSOR_GPS)) {
hottSendGPSResponse();
}
break;
#endif
case 0x8E:
#ifdef HOTT_DEBUG
hottEAMRequests++;
#endif
hottSendEAMResponse();
break;
}
#ifdef HOTT_DEBUG
hottBinaryRequests++;
debug[0] = hottBinaryRequests;
#ifdef GPS
debug[1] = hottGPSRequests;
#endif
debug[2] = hottEAMRequests;
#endif
}
static void flushHottRxBuffer(void)
{
while (serialTotalBytesWaiting(hottPort) > 0) {
serialRead(hottPort);
}
}
static void hottCheckSerialData(uint32_t currentMicros) {
static bool lookingForRequest = true;
uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);
if (bytesWaiting <= 1) {
return;
}
if (bytesWaiting != 2) {
flushHottRxBuffer();
lookingForRequest = true;
return;
}
if (lookingForRequest) {
lastHoTTRequestCheckAt = currentMicros;
lookingForRequest = false;
return;
} else {
bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;
if (!enoughTimePassed) {
return;
}
lookingForRequest = true;
}
uint8_t requestId = serialRead(hottPort);
uint8_t address = serialRead(hottPort);
if (requestId == HOTT_BINARY_MODE_REQUEST_ID) {
processBinaryModeRequest(address);
}
}
static void hottSendTelemetryData(void) {
if (!hottIsSending) {
hottIsSending = true;
serialSetMode(hottPort, MODE_TX);
hottMsgCrc = 0;
return;
}
if (hottMsgRemainingBytesToSendCount == 0) {
hottMsg = NULL;
hottIsSending = false;
serialSetMode(hottPort, MODE_RX);
flushHottRxBuffer();
return;
}
--hottMsgRemainingBytesToSendCount;
if(hottMsgRemainingBytesToSendCount == 0) {
hottSerialWrite(hottMsgCrc++);
return;
}
hottMsgCrc += *hottMsg;
hottSerialWrite(*hottMsg++);
}
static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros)
{
return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ;
}
static inline bool shouldCheckForHoTTRequest()
{
if (hottIsSending) {
return false;
}
return true;
}
void handleHoTTTelemetry(void)
{
static uint32_t serialTimer;
uint32_t now = micros();
if (shouldPrepareHoTTMessages(now)) {
hottPrepareMessages();
lastMessagesPreparedAt = now;
}
if (shouldCheckForHoTTRequest()) {
hottCheckSerialData(now);
}
if (!hottMsg)
return;
if (hottIsSending) {
if(now - serialTimer < HOTT_TX_DELAY_US) {
return;
}
}
hottSendTelemetryData();
serialTimer = now;
}
uint32_t getHoTTTelemetryProviderBaudRate(void) {
return HOTT_BAUDRATE;
}
#endif