mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-09 20:46:07 +01:00
299 lines
11 KiB
C++
299 lines
11 KiB
C++
|
/**
|
||
|
|
******************************************************************************
|
||
|
|
*
|
||
|
|
* @file vehicleconfigurationhelper.cpp
|
||
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||
|
|
* @addtogroup
|
||
|
|
* @{
|
||
|
|
* @addtogroup VehicleConfigurationHelper
|
||
|
|
* @{
|
||
|
|
* @brief
|
||
|
|
*****************************************************************************/
|
||
|
|
/*
|
||
|
|
* 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
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "vehicleconfigurationhelper.h"
|
||
|
|
#include "extensionsystem/pluginmanager.h"
|
||
|
|
#include "hwsettings.h"
|
||
|
|
#include "actuatorsettings.h"
|
||
|
|
#include "attitudesettings.h"
|
||
|
|
#include "mixersettings.h"
|
||
|
|
#include "systemsettings.h"
|
||
|
|
|
||
|
|
VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource)
|
||
|
|
: m_configSource(configSource), m_uavoManager(0)
|
||
|
|
{
|
||
|
|
Q_ASSERT(m_configSource);
|
||
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
||
|
|
m_uavoManager = pm->getObject<UAVObjectManager>();
|
||
|
|
Q_ASSERT(m_uavoManager);
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::setupVehicle()
|
||
|
|
{
|
||
|
|
applyHardwareConfiguration();
|
||
|
|
applyVehicleConfiguration();
|
||
|
|
applyOutputConfiguration();
|
||
|
|
applyFlighModeConfiguration();
|
||
|
|
applyLevellingConfiguration();
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||
|
|
{
|
||
|
|
HwSettings* hwSettings = HwSettings::GetInstance(m_uavoManager);
|
||
|
|
HwSettings::DataFields data = hwSettings->getData();
|
||
|
|
switch(m_configSource->getControllerType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::CONTROLLER_CC:
|
||
|
|
case VehicleConfigurationSource::CONTROLLER_CC3D:
|
||
|
|
// Reset all ports
|
||
|
|
data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLED;
|
||
|
|
data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED;
|
||
|
|
data.CC_MainPort = HwSettings::CC_MAINPORT_DISABLED;
|
||
|
|
switch(m_configSource->getInputType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::INPUT_PWM:
|
||
|
|
data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWM;
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::INPUT_PPM:
|
||
|
|
data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM;
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::INPUT_SBUS:
|
||
|
|
data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS;
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::INPUT_DSM:
|
||
|
|
// TODO: Handle all of the DSM types ?? Which is most common?
|
||
|
|
data.CC_MainPort = HwSettings::CC_MAINPORT_DSM2;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::CONTROLLER_REVO:
|
||
|
|
// TODO: Implement Revo settings
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
hwSettings->setData(data);
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::applyVehicleConfiguration()
|
||
|
|
{
|
||
|
|
|
||
|
|
switch(m_configSource->getVehicleType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::VEHICLE_MULTI:
|
||
|
|
{
|
||
|
|
resetGUIData();
|
||
|
|
resetVehicleConfig();
|
||
|
|
|
||
|
|
switch(m_configSource->getVehicleSubType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y:
|
||
|
|
setupTriCopter();
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS:
|
||
|
|
setupQuadCopter();
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_HEXA:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H:
|
||
|
|
setupHexaCopter();
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V:
|
||
|
|
setupOctoCopter();
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VehicleConfigurationSource::VEHICLE_FIXEDWING:
|
||
|
|
case VehicleConfigurationSource::VEHICLE_HELI:
|
||
|
|
case VehicleConfigurationSource::VEHICLE_SURFACE:
|
||
|
|
// TODO: Implement settings for other vehicle types?
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::applyOutputConfiguration()
|
||
|
|
{
|
||
|
|
ActuatorSettings* actSettings = ActuatorSettings::GetInstance(m_uavoManager);
|
||
|
|
switch(m_configSource->getVehicleType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::VEHICLE_MULTI:
|
||
|
|
{
|
||
|
|
ActuatorSettings::DataFields data = actSettings->getData();
|
||
|
|
|
||
|
|
data.ChannelUpdateFreq[0] = LEGACY_ESC_FREQUENCE;
|
||
|
|
data.ChannelUpdateFreq[1] = LEGACY_ESC_FREQUENCE;
|
||
|
|
data.ChannelUpdateFreq[3] = LEGACY_ESC_FREQUENCE;
|
||
|
|
data.ChannelUpdateFreq[4] = LEGACY_ESC_FREQUENCE;
|
||
|
|
|
||
|
|
qint16 updateFrequence = LEGACY_ESC_FREQUENCE;
|
||
|
|
switch(m_configSource->getESCType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::ESC_LEGACY:
|
||
|
|
updateFrequence = LEGACY_ESC_FREQUENCE;
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::ESC_RAPID:
|
||
|
|
updateFrequence = RAPID_ESC_FREQUENCE;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch(m_configSource->getVehicleSubType())
|
||
|
|
{
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y:
|
||
|
|
data.ChannelUpdateFreq[0] = updateFrequence;
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS:
|
||
|
|
data.ChannelUpdateFreq[0] = updateFrequence;
|
||
|
|
data.ChannelUpdateFreq[1] = updateFrequence;
|
||
|
|
break;
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_HEXA:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS:
|
||
|
|
case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V:
|
||
|
|
data.ChannelUpdateFreq[0] = updateFrequence;
|
||
|
|
data.ChannelUpdateFreq[1] = updateFrequence;
|
||
|
|
data.ChannelUpdateFreq[3] = updateFrequence;
|
||
|
|
data.ChannelUpdateFreq[4] = updateFrequence;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
actSettings->setData(data);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VehicleConfigurationSource::VEHICLE_FIXEDWING:
|
||
|
|
case VehicleConfigurationSource::VEHICLE_HELI:
|
||
|
|
case VehicleConfigurationSource::VEHICLE_SURFACE:
|
||
|
|
// TODO: Implement settings for other vehicle types?
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::applyFlighModeConfiguration()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::applyLevellingConfiguration()
|
||
|
|
{
|
||
|
|
if(m_configSource->isLevellingPerformed())
|
||
|
|
{
|
||
|
|
accelGyroBias bias = m_configSource->getLevellingBias();
|
||
|
|
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(m_uavoManager)->getData();
|
||
|
|
attitudeSettingsData.AccelBias[0] += bias.m_accelerometerXBias;
|
||
|
|
attitudeSettingsData.AccelBias[1] += bias.m_accelerometerYBias;
|
||
|
|
attitudeSettingsData.AccelBias[2] += bias.m_accelerometerZBias;
|
||
|
|
attitudeSettingsData.GyroBias[0] = -bias.m_gyroXBias;
|
||
|
|
attitudeSettingsData.GyroBias[1] = -bias.m_gyroYBias;
|
||
|
|
attitudeSettingsData.GyroBias[2] = -bias.m_gyroZBias;
|
||
|
|
AttitudeSettings::GetInstance(m_uavoManager)->setData(attitudeSettingsData);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::resetVehicleConfig()
|
||
|
|
{
|
||
|
|
// Reset all vehicle data
|
||
|
|
MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager);
|
||
|
|
|
||
|
|
// Reset feed forward, accel times etc
|
||
|
|
mSettings->setFeedForward(0.0f);
|
||
|
|
mSettings->setMaxAccel(1000.0f);
|
||
|
|
mSettings->setAccelTime(0.0f);
|
||
|
|
mSettings->setDecelTime(0.0f);
|
||
|
|
|
||
|
|
// Reset throttle curves
|
||
|
|
QString throttlePattern = "ThrottleCurve%1";
|
||
|
|
for(int i = 1; i <= 2; i++) {
|
||
|
|
UAVObjectField *field = mSettings->getField(throttlePattern.arg(i));
|
||
|
|
Q_ASSERT(field);
|
||
|
|
for(int i = 0; i < field->getNumElements(); i++){
|
||
|
|
field->setValue(i * ( 1.0f / field->getNumElements()), i);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Reset Mixer types and values
|
||
|
|
QString mixerTypePattern = "Mixer%1Type";
|
||
|
|
QString mixerVectorPattern = "Mixer%1Vector";
|
||
|
|
for(int i = 1; i <= 10; i++) {
|
||
|
|
UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i));
|
||
|
|
Q_ASSERT(field);
|
||
|
|
field->setValue(field->getOptions().at(0));
|
||
|
|
|
||
|
|
field = mSettings->getField(mixerVectorPattern.arg(i));
|
||
|
|
Q_ASSERT(field);
|
||
|
|
for(int i = 0; i < field->getNumElements(); i++){
|
||
|
|
field->setValue(0, i);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Apply updates
|
||
|
|
mSettings->setData(mSettings->getData());
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::resetGUIData()
|
||
|
|
{
|
||
|
|
SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager);
|
||
|
|
Q_ASSERT(sSettings);
|
||
|
|
SystemSettings::DataFields data = sSettings->getData();
|
||
|
|
data.AirframeType = SystemSettings::AIRFRAMETYPE_CUSTOM;
|
||
|
|
for(int i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++)
|
||
|
|
{
|
||
|
|
data.GUIConfigData[i] = 0;
|
||
|
|
}
|
||
|
|
sSettings->setData(data);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::setupTriCopter()
|
||
|
|
{
|
||
|
|
// Typical vehicle setup
|
||
|
|
// 1. Setup and apply mixer
|
||
|
|
// 2. Setup GUI data
|
||
|
|
mixerSettings mixer;
|
||
|
|
mixer.channels[0].type = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::setupQuadCopter()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::setupHexaCopter()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
void VehicleConfigurationHelper::setupOctoCopter()
|
||
|
|
{
|
||
|
|
}
|