mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +01:00
Merge branch 'revo-next' of ssh://git.openpilot.org/revo into revo-next
This commit is contained in:
a*morale
commit
75730e7260
@ -44,8 +44,11 @@
|
||||
#include "gps_airspeed.h"
|
||||
#include "baroaltitude.h"
|
||||
#include "baroairspeed.h" // object that will be updated by the module
|
||||
#include "airspeedactual.h" // object that will be updated by the module
|
||||
#include "attitudeactual.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include "baro_airspeed_etasv3.h"
|
||||
#include "baro_airspeed_analog.h"
|
||||
|
||||
// Private constants
|
||||
#if defined (PIOS_INCLUDE_GPS)
|
||||
@ -164,6 +167,7 @@ int32_t AirspeedInitialize()
|
||||
#endif
|
||||
|
||||
BaroAirspeedInitialize();
|
||||
AirspeedActualInitialize();
|
||||
AirspeedSettingsInitialize();
|
||||
|
||||
AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
|
||||
@ -181,7 +185,9 @@ static void airspeedTask(void *parameters)
|
||||
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
|
||||
|
||||
BaroAirspeedData airspeedData;
|
||||
AirspeedActualData airspeedActualData;
|
||||
BaroAirspeedGet(&airspeedData);
|
||||
AirspeedActualGet(&airspeedActualData);
|
||||
|
||||
|
||||
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
@ -205,6 +211,7 @@ static void airspeedTask(void *parameters)
|
||||
{
|
||||
// Update the airspeed object
|
||||
BaroAirspeedGet(&airspeedData);
|
||||
AirspeedActualGet(&airspeedActualData);
|
||||
|
||||
#ifdef BARO_AIRSPEED_PRESENT
|
||||
float airspeed_tas_baro=0;
|
||||
@ -308,7 +315,10 @@ static void airspeedTask(void *parameters)
|
||||
#endif
|
||||
#endif
|
||||
//Set the UAVO
|
||||
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
|
||||
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
|
||||
BaroAirspeedSet(&airspeedData);
|
||||
AirspeedActualSet(&airspeedActualData);
|
||||
|
||||
}
|
||||
}
|
||||
@ -321,6 +331,32 @@ static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
|
||||
}
|
||||
#endif
|
||||
|
||||
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
|
||||
|
||||
//Find out which sensor we're using.
|
||||
switch (airspeedSensorType) {
|
||||
#if defined(PIOS_INCLUDE_MPXV7002)
|
||||
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_MPXV5004)
|
||||
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_MPXV7002) || defined(PIOS_INCLUDE_MPXV5004)
|
||||
//MPXV5004 and MPXV7002 sensors
|
||||
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
|
||||
break;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_ETASV3)
|
||||
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
|
||||
//Eagletree Airspeed v3
|
||||
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
|
||||
@ -36,18 +36,15 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#define AIRSPEED_STORED_IN_A_PROPER_UAVO FALSE //AIRSPEED SHOULD NOT GO INTO BaroAirspeed. IT IS A STATE VARIABLE, AND NOT A MEASUREMENT
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "hwsettings.h"
|
||||
#include "airspeed.h"
|
||||
#include "airspeedsettings.h"
|
||||
#include "baroairspeed.h" // object that will be updated by the module
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004)
|
||||
|
||||
#define SAMPLING_DELAY_MS_ETASV3 50 //Update at 20Hz
|
||||
#define SAMPLING_DELAY_MS_MPXV 10 //Update at 100Hz
|
||||
#define ETS_AIRSPEED_SCALE 1.0f //???
|
||||
#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms]
|
||||
#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms]
|
||||
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO
|
||||
@ -58,29 +55,14 @@
|
||||
|
||||
// Private functions
|
||||
|
||||
#if defined(PIOS_INCLUDE_ETASV3) || defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004)
|
||||
static uint16_t calibrationCount=0;
|
||||
#endif
|
||||
|
||||
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
|
||||
|
||||
#if defined(PIOS_INCLUDE_ETASV3)
|
||||
static uint32_t calibrationSum = 0;
|
||||
#endif
|
||||
|
||||
//Find out which sensor we're using.
|
||||
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){ //MPXV5004 and MPXV7002 sensors
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004)
|
||||
|
||||
void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
|
||||
|
||||
//Ensure that the ADC pin is properly configured
|
||||
if(airspeedADCPin <0){ //It's not, so revert to former sensor type
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
AirspeedSettingsData airspeedSettingsData;
|
||||
AirspeedSettingsGet(&airspeedSettingsData);
|
||||
|
||||
airspeedSettingsData.AirspeedSensorType=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY;
|
||||
|
||||
AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
|
||||
|
||||
return;
|
||||
}
|
||||
@ -93,20 +75,15 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT
|
||||
return;
|
||||
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) { //Then compute the average.
|
||||
calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */
|
||||
AirspeedSettingsData airspeedSettingsData;
|
||||
AirspeedSettingsGet(&airspeedSettingsData);
|
||||
|
||||
|
||||
|
||||
uint16_t sensorCalibration;
|
||||
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
|
||||
uint16_t sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
|
||||
airspeedSettingsData.ZeroPoint=sensorCalibration;
|
||||
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
|
||||
sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
|
||||
PIOS_MPXV7002_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
|
||||
}
|
||||
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
|
||||
uint16_t sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
|
||||
airspeedSettingsData.ZeroPoint=sensorCalibration;
|
||||
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
|
||||
sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
|
||||
PIOS_MPXV5004_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
|
||||
}
|
||||
|
||||
|
||||
@ -114,7 +91,7 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT
|
||||
|
||||
//Set settings UAVO. The airspeed UAVO is set elsewhere in the function.
|
||||
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV)
|
||||
AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
|
||||
AirspeedSettingsZeroPointSet(&sensorCalibration);
|
||||
|
||||
return;
|
||||
}
|
||||
@ -155,69 +132,11 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT
|
||||
|
||||
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
|
||||
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
|
||||
#else
|
||||
//Whoops, no sensor defined in pios_config.h. Revert to GPS.
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
AirspeedSettingsData airspeedSettingsData;
|
||||
AirspeedSettingsGet(&airspeedSettingsData);
|
||||
airspeedSettingsData.AirspeedSensorType=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY;
|
||||
|
||||
AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
|
||||
|
||||
return;
|
||||
#endif
|
||||
}
|
||||
else if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3){ //Eagletree Airspeed v3
|
||||
#if defined(PIOS_INCLUDE_ETASV3)
|
||||
AirspeedSettingsData airspeedSettingsData;
|
||||
AirspeedSettingsGet(&airspeedSettingsData);
|
||||
|
||||
//Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
|
||||
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_ETASV3 / portTICK_RATE_MS);
|
||||
|
||||
//Check to see if airspeed sensor is returning baroAirspeedData
|
||||
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
|
||||
if (baroAirspeedData->SensorValue==-1) {
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
baroAirspeedData->CalibratedAirspeed = 0;
|
||||
BaroAirspeedSet(&baroAirspeedData);
|
||||
return;
|
||||
}
|
||||
|
||||
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
|
||||
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) {
|
||||
calibrationCount++;
|
||||
return;
|
||||
} else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
|
||||
calibrationCount++;
|
||||
calibrationSum += baroAirspeedData->SensorValue;
|
||||
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
|
||||
|
||||
airspeedSettingsData.ZeroPoint=(uint16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f);
|
||||
AirspeedSettingsSet(&airspeedSettingsData);
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
//Compute airspeed
|
||||
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed?
|
||||
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
|
||||
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
|
||||
#else
|
||||
//Whoops, no EagleTree sensor defined in pios_config.h. Declare it unconnected...
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
|
||||
//...and revert to GPS.
|
||||
if (airspeedSensorType!=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) {
|
||||
uint8_t tmpVal=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY;
|
||||
AirspeedSettingsAirspeedSensorTypeSet(&tmpVal); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
109
flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c
Normal file
109
flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c
Normal file
@ -0,0 +1,109 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AirspeedModule Airspeed Module
|
||||
* @brief Communicate with airspeed sensors and return values
|
||||
* @{
|
||||
*
|
||||
* @file baro_airspeed.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Airspeed module, handles temperature and pressure readings from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Output object: BaroAirspeed
|
||||
*
|
||||
* This module will periodically update the value of the BaroAirspeed object.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "hwsettings.h"
|
||||
#include "airspeed.h"
|
||||
#include "airspeedsettings.h"
|
||||
#include "baroairspeed.h" // object that will be updated by the module
|
||||
|
||||
#if defined(PIOS_INCLUDE_ETASV3)
|
||||
|
||||
#define SAMPLING_DELAY_MS_ETASV3 50 //Update at 20Hz
|
||||
#define ETS_AIRSPEED_SCALE 1.0f //???
|
||||
#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms]
|
||||
#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms]
|
||||
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
|
||||
// Private functions
|
||||
|
||||
static uint16_t calibrationCount=0;
|
||||
|
||||
|
||||
void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
|
||||
|
||||
static uint32_t calibrationSum = 0;
|
||||
AirspeedSettingsData airspeedSettingsData;
|
||||
AirspeedSettingsGet(&airspeedSettingsData);
|
||||
|
||||
//Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
|
||||
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_ETASV3 / portTICK_RATE_MS);
|
||||
|
||||
//Check to see if airspeed sensor is returning baroAirspeedData
|
||||
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
|
||||
if (baroAirspeedData->SensorValue==-1) {
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
baroAirspeedData->CalibratedAirspeed = 0;
|
||||
BaroAirspeedSet(&baroAirspeedData);
|
||||
return;
|
||||
}
|
||||
|
||||
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
|
||||
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) {
|
||||
calibrationCount++;
|
||||
return;
|
||||
} else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
|
||||
calibrationCount++;
|
||||
calibrationSum += baroAirspeedData->SensorValue;
|
||||
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
|
||||
|
||||
airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f);
|
||||
AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint );
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
//Compute airspeed
|
||||
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed?
|
||||
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
|
||||
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -0,0 +1,43 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AirspeedModule Airspeed Module
|
||||
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
|
||||
* @{
|
||||
*
|
||||
* @file baro_airspeed_analog.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief Airspeed module, reads temperature and pressure from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ANALOG_AIRSPEED_H
|
||||
#define ANALOG_AIRSPEED_H
|
||||
#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004)
|
||||
|
||||
void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin);
|
||||
|
||||
#endif
|
||||
#endif // ANALOG_AIRSPEED_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -0,0 +1,43 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AirspeedModule Airspeed Module
|
||||
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
|
||||
* @{
|
||||
*
|
||||
* @file baro_airspeed_etasv3.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief Airspeed module, reads temperature and pressure from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ETASV3_AIRSPEED_H
|
||||
#define ETASV3_AIRSPEED_H
|
||||
#if defined(PIOS_INCLUDE_ETASV3)
|
||||
|
||||
void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin);
|
||||
|
||||
#endif
|
||||
#endif // ETASV3_AIRSPEED_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -54,7 +54,7 @@
|
||||
#include "manualcontrol.h"
|
||||
#include "flightstatus.h"
|
||||
#include "pathstatus.h"
|
||||
#include "baroairspeed.h"
|
||||
#include "airspeedactual.h"
|
||||
#include "gpsvelocity.h"
|
||||
#include "gpsposition.h"
|
||||
#include "fixedwingpathfollowersettings.h"
|
||||
@ -91,7 +91,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
|
||||
static void updatePathVelocity();
|
||||
static uint8_t updateFixedDesiredAttitude();
|
||||
static void updateFixedAttitude();
|
||||
static void baroAirspeedUpdatedCb(UAVObjEvent * ev);
|
||||
static void airspeedActualUpdatedCb(UAVObjEvent * ev);
|
||||
static float bound(float val, float min, float max);
|
||||
|
||||
/**
|
||||
@ -125,7 +125,7 @@ int32_t FixedWingPathFollowerInitialize()
|
||||
PathDesiredInitialize();
|
||||
PathStatusInitialize();
|
||||
VelocityDesiredInitialize();
|
||||
BaroAirspeedInitialize();
|
||||
AirspeedActualInitialize();
|
||||
} else {
|
||||
followerEnabled = false;
|
||||
}
|
||||
@ -144,7 +144,7 @@ static float powerIntegral = 0;
|
||||
static float airspeedErrorInt=0;
|
||||
|
||||
// correct speed by measured airspeed
|
||||
static float baroAirspeedBias = 0;
|
||||
static float indicatedAirspeedActualBias = 0;
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
@ -156,7 +156,7 @@ static void pathfollowerTask(void *parameters)
|
||||
|
||||
portTickType lastUpdateTime;
|
||||
|
||||
BaroAirspeedConnectCallback(baroAirspeedUpdatedCb);
|
||||
AirspeedActualConnectCallback(airspeedActualUpdatedCb);
|
||||
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
|
||||
PathDesiredConnectCallback(SettingsUpdatedCb);
|
||||
|
||||
@ -282,7 +282,6 @@ static void updatePathVelocity()
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
|
||||
groundspeed = pathDesired.EndingVelocity;
|
||||
// groundspeed = fixedwingpathfollowerSettings.BestClimbRateSpeed;
|
||||
altitudeSetpoint = pathDesired.End[2];
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
@ -291,16 +290,19 @@ static void updatePathVelocity()
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
bound(progress.fractional_progress,0,1);
|
||||
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
bound(progress.fractional_progress,0,1);
|
||||
break;
|
||||
}
|
||||
// this ensures a significant forward component at least close to the real trajectory
|
||||
if (groundspeed<fixedwingpathfollowerSettings.BestClimbRateSpeed/10.)
|
||||
groundspeed=fixedwingpathfollowerSettings.BestClimbRateSpeed/10.;
|
||||
|
||||
// calculate velocity - can be zero if waypoints are too close
|
||||
VelocityDesiredData velocityDesired;
|
||||
velocityDesired.North = progress.path_direction[0] * fmaxf(groundspeed,1e-6);
|
||||
velocityDesired.East = progress.path_direction[1] * fmaxf(groundspeed,1e-6);
|
||||
velocityDesired.North = progress.path_direction[0] * groundspeed;
|
||||
velocityDesired.East = progress.path_direction[1] * groundspeed;
|
||||
|
||||
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
|
||||
@ -359,17 +361,13 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
|
||||
StabilizationSettingsData stabSettings;
|
||||
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
|
||||
BaroAirspeedData baroAirspeed;
|
||||
AirspeedActualData airspeedActual;
|
||||
|
||||
// float groundspeedActual;
|
||||
float groundspeedActual;
|
||||
float groundspeedDesired;
|
||||
float calibratedAirspeedActual;
|
||||
float airspeedDesired;
|
||||
float indicatedAirspeedActual;
|
||||
float indicatedAirspeedDesired;
|
||||
float airspeedError;
|
||||
float accelActual;
|
||||
float accelDesired;
|
||||
float accelError;
|
||||
float accelCommand;
|
||||
|
||||
float pitchCommand;
|
||||
|
||||
@ -392,7 +390,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
AccelsGet(&accels);
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
BaroAirspeedGet(&baroAirspeed);
|
||||
AirspeedActualGet(&airspeedActual);
|
||||
|
||||
|
||||
/**
|
||||
@ -400,17 +398,20 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
*/
|
||||
|
||||
// Current ground speed
|
||||
// groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
|
||||
calibratedAirspeedActual = baroAirspeed.CalibratedAirspeed;
|
||||
groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
|
||||
// note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
|
||||
// but thanks to accelerometers, groundspeed reacts faster to changes in direction
|
||||
// than airspeed and gps sensors alone
|
||||
indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias;
|
||||
|
||||
// Desired ground speed
|
||||
groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
|
||||
airspeedDesired = bound( groundspeedDesired + baroAirspeedBias,
|
||||
indicatedAirspeedDesired = bound( groundspeedDesired + indicatedAirspeedActualBias,
|
||||
fixedwingpathfollowerSettings.BestClimbRateSpeed,
|
||||
fixedwingpathfollowerSettings.CruiseSpeed);
|
||||
|
||||
// Airspeed error
|
||||
airspeedError = airspeedDesired - calibratedAirspeedActual;
|
||||
airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual;
|
||||
|
||||
// Vertical speed error
|
||||
descentspeedDesired = bound (
|
||||
@ -421,35 +422,35 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
|
||||
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
|
||||
if (groundspeedDesired - baroAirspeedBias <= 0 ) {
|
||||
if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) {
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
|
||||
result = 0;
|
||||
}
|
||||
// Error condition: plane too slow or too fast
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
|
||||
if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) {
|
||||
if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) {
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
|
||||
result = 0;
|
||||
}
|
||||
if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
|
||||
if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
|
||||
result = 0;
|
||||
}
|
||||
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
|
||||
if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
|
||||
result = 0;
|
||||
}
|
||||
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
|
||||
if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
|
||||
result = 0;
|
||||
}
|
||||
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
|
||||
if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
if (calibratedAirspeedActual<1e-6) {
|
||||
if (indicatedAirspeedActual<1e-6) {
|
||||
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
|
||||
// also we cannot handle planes flying backwards, lets just wait until the nose drops
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
|
||||
@ -473,47 +474,14 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
|
||||
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]
|
||||
)*(1.0f-1.0f/(1.0f+30.0f/dT));
|
||||
}
|
||||
} else powerIntegral = 0;
|
||||
|
||||
// Compute final throttle response
|
||||
powerCommand = (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
|
||||
powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL];
|
||||
|
||||
if (0) {
|
||||
//Saturate command, and reduce integral as a way of further avoiding integral windup
|
||||
if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) {
|
||||
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
|
||||
powerIntegral = bound(
|
||||
powerIntegral -
|
||||
( powerCommand
|
||||
- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]
|
||||
)/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
|
||||
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
|
||||
}
|
||||
|
||||
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX];
|
||||
}
|
||||
if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) {
|
||||
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
|
||||
powerIntegral = bound(
|
||||
powerIntegral +
|
||||
( powerCommand
|
||||
- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]
|
||||
)/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
|
||||
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Saturate throttle command.
|
||||
if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) {
|
||||
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX];
|
||||
}
|
||||
if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) {
|
||||
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN];
|
||||
}
|
||||
powerCommand = bound(
|
||||
(powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
|
||||
powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL],
|
||||
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
|
||||
|
||||
//Output internal state to telemetry
|
||||
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = powerError;
|
||||
@ -552,66 +520,28 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
/**
|
||||
* Compute desired pitch command
|
||||
*/
|
||||
// compute desired acceleration
|
||||
if(0){
|
||||
accelDesired = bound( (airspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
|
||||
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
|
||||
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
|
||||
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
|
||||
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = 0.0f;
|
||||
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = accelDesired;
|
||||
|
||||
// exclude gravity from acceleration. If the aicraft is flying at high pitch, it fights gravity without getting faster
|
||||
accelActual = accels.x - (sinf( DEG2RAD * attitudeActual.Pitch) * GEE);
|
||||
|
||||
accelError = accelDesired - accelActual;
|
||||
if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
|
||||
accelIntegral = bound(accelIntegral + accelError * dT,
|
||||
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
|
||||
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
|
||||
}
|
||||
|
||||
accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] +
|
||||
accelIntegral*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
|
||||
if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){
|
||||
//Integrate with saturation
|
||||
airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
|
||||
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
|
||||
}
|
||||
|
||||
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = accelError;
|
||||
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = accelIntegral;
|
||||
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = accelCommand;
|
||||
|
||||
pitchCommand= -accelCommand + bound ( (-descentspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
|
||||
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
|
||||
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
|
||||
);
|
||||
}
|
||||
else {
|
||||
|
||||
if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
|
||||
//Integrate with saturation
|
||||
airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
|
||||
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
|
||||
}
|
||||
|
||||
//Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
|
||||
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
|
||||
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
|
||||
);
|
||||
|
||||
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
|
||||
pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP]
|
||||
+ airspeedErrorInt*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]
|
||||
) + verticalSpeedToPitchCommandComponent;
|
||||
|
||||
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
|
||||
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
|
||||
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = airspeedDesired;
|
||||
|
||||
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = -123;
|
||||
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = -123;
|
||||
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = pitchCommand;
|
||||
|
||||
}
|
||||
//Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
|
||||
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
|
||||
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
|
||||
);
|
||||
|
||||
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
|
||||
pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP]
|
||||
+ airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
|
||||
) + verticalSpeedToPitchCommandComponent;
|
||||
|
||||
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
|
||||
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
|
||||
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
|
||||
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
|
||||
pitchCommand,
|
||||
@ -665,7 +595,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
/**
|
||||
* Compute desired yaw command
|
||||
*/
|
||||
// TODO implement raw control mode for yaw and base on Accels.X
|
||||
// TODO implement raw control mode for yaw and base on Accels.Y
|
||||
stabDesired.Yaw = 0;
|
||||
|
||||
|
||||
@ -700,21 +630,19 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
PathDesiredGet(&pathDesired);
|
||||
}
|
||||
|
||||
static void baroAirspeedUpdatedCb(UAVObjEvent * ev)
|
||||
static void airspeedActualUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
|
||||
BaroAirspeedData baroAirspeed;
|
||||
AirspeedActualData airspeedActual;
|
||||
VelocityActualData velocityActual;
|
||||
|
||||
BaroAirspeedGet(&baroAirspeed);
|
||||
if (baroAirspeed.BaroConnected != BAROAIRSPEED_BAROCONNECTED_TRUE && BaroAirspeedReadOnly()) {
|
||||
baroAirspeedBias = 0;
|
||||
} else {
|
||||
VelocityActualGet(&velocityActual);
|
||||
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
|
||||
AirspeedActualGet(&airspeedActual);
|
||||
VelocityActualGet(&velocityActual);
|
||||
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
|
||||
|
||||
|
||||
baroAirspeedBias = baroAirspeed.TrueAirspeed - groundspeed;
|
||||
}
|
||||
|
||||
indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed;
|
||||
// note - we do fly by Indicated Airspeed (== calibrated airspeed)
|
||||
// however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
|
||||
|
||||
}
|
||||
|
||||
@ -39,6 +39,7 @@ UAVOBJSRCFILENAMES += magbias
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += baroairspeed
|
||||
UAVOBJSRCFILENAMES += airspeedsettings
|
||||
UAVOBJSRCFILENAMES += airspeedactual
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
UAVOBJSRCFILENAMES += firmwareiapobj
|
||||
UAVOBJSRCFILENAMES += flightbatterystate
|
||||
|
||||
@ -37,6 +37,8 @@ UAVOBJSRCFILENAMES += accels
|
||||
UAVOBJSRCFILENAMES += magnetometer
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += baroairspeed
|
||||
UAVOBJSRCFILENAMES += airspeedsettings
|
||||
UAVOBJSRCFILENAMES += airspeedactual
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
UAVOBJSRCFILENAMES += firmwareiapobj
|
||||
UAVOBJSRCFILENAMES += flightbatterystate
|
||||
|
||||
@ -6,9 +6,9 @@
|
||||
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="20"/>
|
||||
<!-- Vne, i.e. maximum airspeed the airframe can handle -->
|
||||
<field name="CruiseSpeed" units="m/s" type="float" elements="1" defaultvalue="20"/>
|
||||
<!-- Cruise speed the in level flight - leave some safety margin -->
|
||||
<!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
|
||||
<field name="BestClimbRateSpeed" units="m/s" type="float" elements="1" defaultvalue="10"/>
|
||||
<!-- V_y, i.e. airspeed at which plane climbs the best -->
|
||||
<!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
|
||||
<field name="StallSpeedClean" units="m/s" type="float" elements="1" defaultvalue="8"/>
|
||||
<!-- Vs1, i.e. stall speed in clean configuration- leave some safety margin -->
|
||||
<field name="StallSpeedDirty" units="m/s" type="float" elements="1" defaultvalue="8"/>
|
||||
@ -28,12 +28,9 @@
|
||||
<field name="BearingPI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
|
||||
<!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading -->
|
||||
|
||||
<field name="SpeedP" units="(m/s^2) / ((m/s)/(m/s)" type="float" elementnames="Kp,Max" defaultvalue="10,10"/>
|
||||
<!-- coefficients for desired acceleration
|
||||
in relation to relative airspeed error (IASerror/IASactual)-->
|
||||
<field name="AccelPI" units="deg / (m/s^2)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
|
||||
<field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
|
||||
<!-- coefficients for desired pitch
|
||||
in relation to acceleration error -->
|
||||
in relation to relative speed error (IASerror/IASactual) -->
|
||||
<field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
|
||||
<!-- coefficients for adjusting desired pitch
|
||||
in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->
|
||||
|
||||
@ -1,9 +1,9 @@
|
||||
<xml>
|
||||
<object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
|
||||
<description>Object Storing Debugging Information on PID internals</description>
|
||||
<field name="Error" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
|
||||
<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
|
||||
<field name="Command" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
|
||||
<field name="Error" units="" type="float" elementnames="Bearing,Speed,Power" />
|
||||
<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Power" />
|
||||
<field name="Command" units="" type="float" elementnames="Bearing,Speed,Power" />
|
||||
<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" />
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user