rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1216 some fixes to flightmode and arm state management, update GCS control widget to be able to test and debug new control infrastructure

Browse Source
This commit is contained in:
Corvus Corax 2014-03-02 22:20:18 +01:00
parent 12eed41e58
commit 2c99b008c6
6 changed files with 54 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/modules/ManualControl/armhandler.c
View File

@ -90,11 +90,11 @@ void armHandler(bool newinit)
armSwitch = true;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
AccessoryDesiredInstGet(0, &acc);
AccessoryDesiredInstGet(1, &acc);
armSwitch = true;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
AccessoryDesiredInstGet(0, &acc);
AccessoryDesiredInstGet(2, &acc);
armSwitch = true;
break;
default:
@ -298,7 +298,7 @@ static bool forcedDisArm(void)
if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
if (alarms.ManualControl == SYSTEMALARMS_ALARM_CRITICAL) {
if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;

9
flight/modules/ManualControl/manualcontrol.c
View File

@ -193,7 +193,7 @@ static void manualControlTask(void)
// Depending on the mode update the Stabilization or Actuator objects
controlHandler *handler = &handler_MANUAL;
switch (flightStatus.FlightMode) {
switch (newMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
handler = &handler_MANUAL;
break;
@ -223,7 +223,12 @@ static void manualControlTask(void)
}
bool newinit = false;
if (flightStatus.FlightMode != newMode) {
// FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
static bool firstRun = true;
if (flightStatus.FlightMode != newMode || firstRun) {
firstRun = false;
flightStatus.ControlChain = handler->controlChain;
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);

4
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
View File

@ -52,12 +52,12 @@
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBoxArmed">
<widget class="QCheckBox" name="checkBoxArming">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string>Armed</string>
<string>Arm switch (Accessory0)</string>
</property>
</widget>
</item>

17
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
View File

@ -231,7 +231,9 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r
}
if (buttonThrottleControl == 0) {
obj->getField("Throttle")->setDouble(newThrottle);
obj->getField("Thrust")->setDouble(newThrottle);
}
obj->getField("Connected")->setValue("True");
obj->updated();
}
}
@ -308,9 +310,11 @@ void GCSControlGadget::readUDPCommand()
}
if (throttle != obj->getField("Throttle")->getDouble()) {
obj->getField("Throttle")->setDouble(constrain(throttle));
obj->getField("Thrust")->setDouble(constrain(throttle));
update = true;
}
if (update) {
obj->getField("Connected")->setValue("True");
obj->updated();
}
}
@ -354,6 +358,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
break;
case 4: // Throttle
obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount));
obj->getField("Thrust")->setValue(bound(obj->getField("Thrust")->getValue().toDouble() + buttonSettings[number].Amount));
break;
}
}
@ -372,6 +377,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
break;
case 4: // Throttle
obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount));
obj->getField("Thrust")->setValue(bound(obj->getField("Thrust")->getValue().toDouble() - buttonSettings[number].Amount));
break;
}
}
@ -380,14 +386,10 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
switch (buttonSettings[number].FunctionID) {
case 1: // Armed
if (currentCGSControl) {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
if (obj->getField("Armed")->getValue().toString().compare("Armed") == 0) {
obj->getField("Armed")->setValue("Disarmed");
if (((GCSControlGadgetWidget *)m_widget)->getArmed()) {
((GCSControlGadgetWidget *)m_widget)->setArmed(false);
} else {
obj->getField("Armed")->setValue("Armed");
((GCSControlGadgetWidget *)m_widget)->setArmed(true);
}
}
break;
@ -407,6 +409,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
break;
}
obj->getField("Connected")->setValue("True");
obj->updated();
}
// buttonSettings[number].ActionID NIDT

48
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
View File

@ -53,8 +53,13 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY);
// Set up the drop down box for the flightmode
UAVDataObject *flightStatus = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions());
// TODO: update this with named modes based on current configuration
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 1");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 2");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 3");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 4");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 5");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 6");
// Set up slots and signals for joysticks
connect(m_gcscontrol->widgetLeftStick, SIGNAL(positionClicked(double, double)), this, SLOT(leftStickClicked(double, double)));
@ -62,7 +67,7 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
// Connect misc controls
connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int)));
connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
connect(m_gcscontrol->checkBoxArming, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)), this, SLOT(toggleUDPControl(int))); // UDP control checkbox
@ -117,16 +122,17 @@ void GCSControlGadgetWidget::toggleControl(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVDataObject *obj2 = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("AccessoryDesired"), 0));
UAVObject::Metadata mdata = obj->getMetadata();
UAVObject::Metadata mdata = obj->getMetadata();
if (state) {
mccInitialData = mdata;
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
UAVObject::SetGcsTelemetryAcked(mdata, false);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.gcsTelemetryUpdatePeriod = 100;
m_gcscontrol->checkBoxUDPControl->setEnabled(true);
} else {
@ -135,18 +141,19 @@ void GCSControlGadgetWidget::toggleControl(int state)
m_gcscontrol->checkBoxUDPControl->setEnabled(false);
}
obj->setMetadata(mdata);
obj2->setMetadata(mdata);
}
void GCSControlGadgetWidget::toggleArmed(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("AccessoryDesired"), 0));
if (state) {
obj->getField("Armed")->setValue("Armed");
obj->getField("AccessoryVal")->setValue(1);
} else {
obj->getField("Armed")->setValue("Disarmed");
obj->getField("AccessoryVal")->setValue(-1);
}
obj->updated();
}
@ -155,11 +162,7 @@ void GCSControlGadgetWidget::mccChanged(UAVObject *obj)
{
Q_UNUSED(obj);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *flightStatus = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString()));
m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
m_gcscontrol->comboBoxFlightMode->setCurrentIndex(obj->getField("FlightModeSwitchPosition")->getValue().toInt());
}
void GCSControlGadgetWidget::toggleUDPControl(int state)
@ -178,10 +181,9 @@ void GCSControlGadgetWidget::selectFlightMode(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
UAVObjectField *field = obj->getField("FlightMode");
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
field->setValue(field->getOptions()[state]);
obj->getField("FlightModeSwitchPosition")->setValue(state);
obj->updated();
}
@ -204,6 +206,16 @@ bool GCSControlGadgetWidget::getUDPControl(void)
return m_gcscontrol->checkBoxUDPControl->isChecked();
}
void GCSControlGadgetWidget::setArmed(bool newState)
{
m_gcscontrol->checkBoxArming->setChecked(newState);
}
bool GCSControlGadgetWidget::getArmed(void)
{
return m_gcscontrol->checkBoxArming->isChecked();
}
/**
* @}

2
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
View File

@ -45,6 +45,8 @@ public:
bool getGCSControl(void);
void setUDPControl(bool newState);
bool getUDPControl(void);
void setArmed(bool newState);
bool getArmed(void);
signals:
void sticksChanged(double leftX, double leftY, double rightX, double rightY);