From e2819c6815f5b2321c4be276d45c21df760d1e80 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 7 May 2011 12:43:13 -0500 Subject: [PATCH 01/20] Restructure the ManualControlCommand code to be much more readable. Also facilitates the new FlightMode object. --- flight/Modules/ManualControl/manualcontrol.c | 368 ++++++++----------- 1 file changed, 160 insertions(+), 208 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 98996ee1e..2313aed2f 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -54,6 +54,7 @@ #define THROTTLE_FAILSAFE -0.1 #define FLIGHT_MODE_LIMIT 1.0/3.0 #define ARMED_TIME_MS 1000 +#define ARMED_THRESHOLD 0.50 //safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 150 @@ -70,13 +71,16 @@ typedef enum // Private variables static xTaskHandle taskHandle; static ArmState_t armState; +static portTickType lastSysTime; // Private functions static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode); +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void manualControlTask(void *parameters); -static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent); +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); @@ -159,13 +163,11 @@ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; - portTickType lastSysTime; - + float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; - enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); @@ -211,7 +213,7 @@ static void manualControlTask(void *parameters) #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0); + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Check settings, if error raise alarm @@ -227,38 +229,23 @@ static void manualControlTask(void *parameters) } // decide if we have valid manual input or not - bool valid_input_detected = TRUE; - if (!validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle])) - valid_input_detected = FALSE; - if (!validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll])) - valid_input_detected = FALSE; - if (!validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw])) - valid_input_detected = FALSE; - if (!validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch])) - valid_input_detected = FALSE; + bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && + validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && + validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && + validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); // Implement hysteresis loop on connection status - if (valid_input_detected) - { - if (++connected_count > 10) - { - connection_state = CONNECTED; - connected_count = 0; - disconnected_count = 0; - } - } - else - { - if (++disconnected_count > 10) - { - connection_state = DISCONNECTED; - connected_count = 0; - disconnected_count = 0; - } + if (valid_input_detected && (++connected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; + disconnected_count = 0; + } else if (!valid_input_detected && (++disconnected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; + disconnected_count = 0; } - if (connection_state == DISCONNECTED) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; @@ -269,161 +256,22 @@ static void manualControlTask(void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[settings.Roll]; - cmd.Pitch = scaledChannel[settings.Pitch]; - cmd.Yaw = scaledChannel[settings.Yaw]; - cmd.Throttle = scaledChannel[settings.Throttle]; - flightMode = scaledChannel[settings.FlightMode]; + cmd.Roll = scaledChannel[settings.Roll]; + cmd.Pitch = scaledChannel[settings.Pitch]; + cmd.Yaw = scaledChannel[settings.Yaw]; + cmd.Throttle = scaledChannel[settings.Throttle]; + flightMode = scaledChannel[settings.FlightMode]; - if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) - cmd.Accessory1 = scaledChannel[settings.Accessory1]; - else - cmd.Accessory1 = 0; + // Set accessory channels + cmd.Accessory1 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) ? scaledChannel[settings.Accessory1] : 0; + cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0; + cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0; - if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) - cmd.Accessory2 = scaledChannel[settings.Accessory2]; - else - cmd.Accessory2 = 0; - - if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) - cmd.Accessory3 = scaledChannel[settings.Accessory3]; - else - cmd.Accessory3 = 0; - - // Note here the code is ass - if (flightMode < -FLIGHT_MODE_LIMIT) - cmd.FlightMode = settings.FlightModePosition[0]; - else if (flightMode > FLIGHT_MODE_LIMIT) - cmd.FlightMode = settings.FlightModePosition[2]; - else - cmd.FlightMode = settings.FlightModePosition[1]; - - - // - // Arming and Disarming mechanism - // - - if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { - // In this configuration we always disarm - cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; - } else { - // In all other cases, we will not change the arm state when disconnected - if (connection_state == CONNECTED) - { - if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { - // In this configuration, we go into armed state as soon as the throttle is low, never disarm - if (cmd.Throttle < 0) { - cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; - } - } else { - // When the configuration is not "Always armed" and no "Always disarmed", - // the state will not be changed when the throttle is not low - if (cmd.Throttle < 0) { - static portTickType armedDisarmStart; - float armingInputLevel = 0; - - // Calc channel see assumptions7 - switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { - case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break; - case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break; - case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break; - } - - bool manualArm = false; - bool manualDisarm = false; - - if (connection_state == CONNECTED) { - // Should use RC input only if RX is connected - if (armingInputLevel <= -0.50) - manualArm = true; - else if (armingInputLevel >= +0.50) - manualDisarm = true; - } - - // Swap arm-disarming see assumptions8 - if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) { - bool temp = manualArm; - manualArm = manualDisarm; - manualDisarm = temp; - } - - switch(armState) { - case ARM_STATE_DISARMED: - cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; - - if (manualArm) - { - if (okToArm()) // only allow arming if it's OK too - { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_ARMING_MANUAL; - } - } - break; - - case ARM_STATE_ARMING_MANUAL: - if (manualArm) { - if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) - armState = ARM_STATE_ARMED; - } - else - armState = ARM_STATE_DISARMED; - break; - - case ARM_STATE_ARMED: - // When we get here, the throttle is low, - // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; - break; - - case ARM_STATE_DISARMING_TIMEOUT: - // We get here when armed while throttle low, even when the arming timeout is not enabled - if (settings.ArmedTimeout != 0) - if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) - armState = ARM_STATE_DISARMED; - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; - - case ARM_STATE_DISARMING_MANUAL: - if (manualDisarm) { - if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) - armState = ARM_STATE_DISARMED; - } - else - armState = ARM_STATE_ARMED; - break; - } // End Switch - } else { - // The throttle is not low, in case we where arming or disarming, abort - switch(armState) { - case ARM_STATE_DISARMING_MANUAL: - case ARM_STATE_DISARMING_TIMEOUT: - armState = ARM_STATE_ARMED; - break; - case ARM_STATE_ARMING_MANUAL: - armState = ARM_STATE_DISARMED; - break; - default: - // Nothing needs to be done in the other states - break; - } - } - } - } - } - // - // End of arming/disarming - // + processFlightMode(&cmd, &settings, flightMode); + processArm(&cmd, &settings); // Update cmd object ManualControlCommandSet(&cmd); @@ -517,7 +365,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ -static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent) +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) { float valueScaled; @@ -537,20 +385,6 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr valueScaled = 0; } - // Neutral RC stick position dead band - if (deadband_percent > 0) - { - if (deadband_percent > 50) deadband_percent = 50; // limit deadband to a maximum of 50% - float deadband = (float)deadband_percent / 100; - if (fabs(valueScaled) <= deadband) - valueScaled = 0; // deadband the value - else - if (valueScaled < 0) - valueScaled = (valueScaled + deadband) / (1.0 - deadband); // value scales 0.0 to -1.0 after deadband - else - valueScaled = (valueScaled - deadband) / (1.0 - deadband); // value scales 0.0 to +1.0 after deadband - } - // Bound if (valueScaled > 1.0) valueScaled = 1.0; else @@ -591,6 +425,134 @@ static bool okToArm(void) return true; } +/** + * @brief Process the inputs and determine whether to arm or not + * @param[out] cmd The structure to set the armed in + * @param[in] settings Settings indicating the necessary position + */ +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +{ + bool lowThrottle = cmd->Throttle <= 0; + + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { + // In this configuration we always disarm + cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; + } else { + // Not really needed since this function not called when disconnected + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + return; + + // The throttle is not low, in case we where arming or disarming, abort + if (!lowThrottle) { + switch(armState) { + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; + } + return; + } + + // The rest of these cases throttle is low + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { + // In this configuration, we go into armed state as soon as the throttle is low, never disarm + cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; + return; + } + + + // When the configuration is not "Always armed" and no "Always disarmed", + // the state will not be changed when the throttle is not low + static portTickType armedDisarmStart; + float armingInputLevel = 0; + + // Calc channel see assumptions7 + int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; + switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { + case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; + case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; + case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; + } + + bool manualArm = false; + bool manualDisarm = false; + + if (armingInputLevel <= -ARMED_THRESHOLD) + manualArm = true; + else if (armingInputLevel >= +ARMED_THRESHOLD) + manualDisarm = true; + + switch(armState) { + case ARM_STATE_DISARMED: + cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; + + // only allow arming if it's OK too + if (manualArm && okToArm()) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + break; + + case ARM_STATE_ARMING_MANUAL: + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_ARMED; + else if (!manualArm) + armState = ARM_STATE_DISARMED; + break; + + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; + break; + + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) + armState = ARM_STATE_DISARMED; + + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; + + case ARM_STATE_DISARMING_MANUAL: + if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_DISARMED; + else if (!manualDisarm) + armState = ARM_STATE_ARMED; + break; + } // End Switch + } +} + +/** + * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly + * @param[out] cmd Pointer to the command structure to set the flight mode in + * @param[in] settings The settings which indicate which position is which mode + * @param[in] flightMode the value of the switch position + */ +static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode) +{ + // Note here the code is ass + if (flightMode < -FLIGHT_MODE_LIMIT) + cmd->FlightMode = settings->FlightModePosition[0]; + else if (flightMode > FLIGHT_MODE_LIMIT) + cmd->FlightMode = settings->FlightModePosition[2]; + else + cmd->FlightMode = settings->FlightModePosition[1]; +} + /** * @brief Determine if the manual input value is within acceptable limits * @returns return TRUE if so, otherwise return FALSE @@ -606,16 +568,6 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); } -// -//static void armingMechanism(uint8_t* armingState, const ManualControlSettingsData* settings, const ManualControlCommandData* cmd) -//{ -// if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { -// *armingState = MANUALCONTROLCOMMAND_ARMED_FALSE; -// return; -// } -// -// -//} /** * @} * @} From ae841d57cb63532ce8d9101d8d625182a0978f1f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 3 May 2011 00:52:04 -0500 Subject: [PATCH 02/20] OP-475: New FlightStatus object. Removed from ManualControlCommand. Won't build yet. --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ shared/uavobjectdefinition/flightstatus.xml | 14 ++++++++++++++ .../uavobjectdefinition/manualcontrolcommand.xml | 5 +---- 3 files changed, 17 insertions(+), 4 deletions(-) create mode 100644 shared/uavobjectdefinition/flightstatus.xml diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 5b209188a..9a4729495 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -9,6 +9,7 @@ /* Begin PBXFileReference section */ 65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = ""; }; 6502584212CA4D2600583CDF /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps13state.c; path = ../../AHRS/insgps13state.c; sourceTree = SOURCE_ROOT; }; + 65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = ""; }; 6509C7E912CA57DC002E5DC2 /* insgps16state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps16state.c; path = ../../AHRS/insgps16state.c; sourceTree = SOURCE_ROOT; }; 650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = ""; }; 650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = ""; }; @@ -7410,6 +7411,7 @@ 65C35E7712EFB2F3004811C2 /* velocityactual.xml */, 65C35E7812EFB2F3004811C2 /* velocitydesired.xml */, 65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */, + 65078B09136FCEE600536549 /* flightstatus.xml */, ); path = uavobjectdefinition; sourceTree = ""; diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml new file mode 100644 index 000000000..849e458d6 --- /dev/null +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -0,0 +1,14 @@ + + + Contains major flight status information for other modules. + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index f5241ad9f..cab420183 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -7,10 +7,7 @@ - - - - + From 80c839d5bb9cbc28e9d45a52d13dd27c10749c47 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 3 May 2011 11:04:44 -0500 Subject: [PATCH 03/20] OP-475: Starting to use the new FlightStatus object --- flight/CopterControl/Makefile | 1 + flight/Modules/Actuator/actuator.c | 11 ++- .../Modules/ManualControl/inc/manualcontrol.h | 12 +-- flight/Modules/ManualControl/manualcontrol.c | 84 +++++++++++++------ flight/Modules/Stabilization/stabilization.c | 24 +++--- flight/Modules/System/systemmod.c | 8 +- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- .../manualcontrolcommand.xml | 1 - 8 files changed, 88 insertions(+), 55 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 0c7d579f2..50cf10bc6 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -160,6 +160,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/objectpersistence.c SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c +SRC += $(OPUAVSYNTHDIR)/flightstatus.c SRC += $(OPUAVSYNTHDIR)/systemstats.c SRC += $(OPUAVSYNTHDIR)/systemalarms.c SRC += $(OPUAVSYNTHDIR)/systemsettings.c diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 69a29062f..c0a52b426 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -37,7 +37,7 @@ #include "systemsettings.h" #include "actuatordesired.h" #include "actuatorcommand.h" -#include "manualcontrolcommand.h" +#include "flightstatus.h" #include "mixersettings.h" #include "mixerstatus.h" @@ -135,7 +135,7 @@ static void actuatorTask(void* parameters) MixerSettingsData mixerSettings; ActuatorDesiredData desired; MixerStatusData mixerStatus; - ManualControlCommandData manualControl; + FlightStatusData flightStatus; ActuatorSettingsGet(&settings); PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); @@ -165,7 +165,7 @@ static void actuatorTask(void* parameters) lastSysTime = thisSysTime; - ManualControlCommandGet(&manualControl); + FlightStatusGet(&flightStatus); SystemSettingsGet(&sysSettings); MixerStatusGet(&mixerStatus); MixerSettingsGet (&mixerSettings); @@ -190,10 +190,9 @@ static void actuatorTask(void* parameters) AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - bool armed = manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE; + bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool positiveThrottle = desired.Throttle >= 0.00; - bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE && - manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE; + bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1); float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2); diff --git a/flight/Modules/ManualControl/inc/manualcontrol.h b/flight/Modules/ManualControl/inc/manualcontrol.h index 2b54345ac..608c12d1d 100644 --- a/flight/Modules/ManualControl/inc/manualcontrol.h +++ b/flight/Modules/ManualControl/inc/manualcontrol.h @@ -35,12 +35,12 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path; #define PARSE_FLIGHT_MODE(x) ( \ - (x == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ - (x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ - (x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ - (x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ - (x == MANUALCONTROLCOMMAND_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ - (x == MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \ + (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \ ) int32_t ManualControlInitialize(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 2313aed2f..0fccd9759 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -41,6 +41,7 @@ #include "actuatordesired.h" #include "stabilizationdesired.h" #include "flighttelemetrystats.h" +#include "flightstatus.h" // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -76,7 +77,7 @@ static portTickType lastSysTime; // Private functions static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); -static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode); +static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void manualControlTask(void *parameters); @@ -129,12 +130,12 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value); #define assumptions_flightmode ( \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_VELOCITYCONTROL) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD) \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ ) #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) @@ -163,7 +164,7 @@ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; - + FlightStatusData flightStatus; float flightMode = 0; uint8_t disconnected_count = 0; @@ -171,8 +172,8 @@ static void manualControlTask(void *parameters) // Make sure unarmed on power up ManualControlCommandGet(&cmd); - cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; - ManualControlCommandSet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; armState = ARM_STATE_DISARMED; // Main task loop @@ -270,7 +271,7 @@ static void manualControlTask(void *parameters) cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0; cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0; - processFlightMode(&cmd, &settings, flightMode); + processFlightMode(&settings, flightMode); processArm(&cmd, &settings); // Update cmd object @@ -283,8 +284,10 @@ static void manualControlTask(void *parameters) } + FlightStatusGet(&flightStatus); + // Depending on the mode update the Stabilization or Actuator objects - switch(PARSE_FLIGHT_MODE(cmd.FlightMode)) { + switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { case FLIGHTMODE_UNDEFINED: // This reflects a bug in the code architecture! AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); @@ -298,7 +301,7 @@ static void manualControlTask(void *parameters) case FLIGHTMODE_GUIDANCE: // TODO: Implement break; - } + } } } @@ -322,14 +325,16 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon StabilizationSettingsGet(&stabSettings); uint8_t * stab_settings; - switch(cmd->FlightMode) { - case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1: + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch(flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = settings->Stabilization1Settings; break; - case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: stab_settings = settings->Stabilization2Settings; break; - case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: stab_settings = settings->Stabilization3Settings; break; default: @@ -425,18 +430,33 @@ static bool okToArm(void) return true; } +/** + * @brief Update the flightStatus object only if value changed. Reduces callbacks + * @param[in] val The new value + */ +static void setArmedIfChanged(uint8_t val) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + if(flightStatus.Armed != val) { + flightStatus.Armed = val; + FlightStatusSet(&flightStatus); + } +} + /** * @brief Process the inputs and determine whether to arm or not * @param[out] cmd The structure to set the armed in * @param[in] settings Settings indicating the necessary position */ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) -{ +{ + bool lowThrottle = cmd->Throttle <= 0; if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm - cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else { // Not really needed since this function not called when disconnected if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) @@ -462,7 +482,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData // The rest of these cases throttle is low if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm - cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); return; } @@ -490,7 +510,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData switch(armState) { case ARM_STATE_DISARMED: - cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); // only allow arming if it's OK too if (manualArm && okToArm()) { @@ -500,6 +520,8 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData break; case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) armState = ARM_STATE_ARMED; else if (!manualArm) @@ -511,7 +533,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; - cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); break; case ARM_STATE_DISARMING_TIMEOUT: @@ -542,15 +564,25 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * @param[in] settings The settings which indicate which position is which mode * @param[in] flightMode the value of the switch position */ -static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode) +static void processFlightMode(ManualControlSettingsData * settings, float flightMode) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + uint8_t newMode; // Note here the code is ass if (flightMode < -FLIGHT_MODE_LIMIT) - cmd->FlightMode = settings->FlightModePosition[0]; + newMode = settings->FlightModePosition[0]; else if (flightMode > FLIGHT_MODE_LIMIT) - cmd->FlightMode = settings->FlightModePosition[2]; + newMode = settings->FlightModePosition[2]; else - cmd->FlightMode = settings->FlightModePosition[1]; + newMode = settings->FlightModePosition[1]; + + if(flightStatus.FlightMode != newMode) { + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + } + } /** diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 73f2f39db..70b9fa1ae 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -39,9 +39,10 @@ #include "stabilizationdesired.h" #include "attitudeactual.h" #include "attituderaw.h" -#include "manualcontrolcommand.h" +#include "flightstatus.h" #include "systemsettings.h" #include "ahrssettings.h" +#include "manualcontrol.h" // Just to get a macro #include "CoordinateConversions.h" // Private constants @@ -125,7 +126,7 @@ static void stabilizationTask(void* parameters) AttitudeActualData attitudeActual; AttitudeRawData attitudeRaw; SystemSettingsData systemSettings; - ManualControlCommandData manualControl; + FlightStatusData flightStatus; SettingsUpdatedCb((UAVObjEvent *) NULL); @@ -142,19 +143,26 @@ static void stabilizationTask(void* parameters) continue; } + // Clear alarms + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + + // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; - ManualControlCommandGet(&manualControl); + FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); RateDesiredGet(&rateDesired); SystemSettingsGet(&systemSettings); - + + if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) + return; + #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; @@ -261,10 +269,6 @@ static void stabilizationTask(void* parameters) // Save dT actuatorDesired.UpdateTime = dT * 1000; - if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) - { - shouldUpdate = 0; - } if(shouldUpdate) @@ -275,14 +279,12 @@ static void stabilizationTask(void* parameters) ActuatorDesiredSet(&actuatorDesired); } - if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE || + if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || !shouldUpdate || (stabDesired.Throttle < 0)) { ZeroPids(); } - // Clear alarms - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } } diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index a3785dac6..6c5a72d16 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -41,7 +41,7 @@ #include "openpilot.h" #include "systemmod.h" #include "objectpersistence.h" -#include "manualcontrolcommand.h" +#include "flightstatus.h" #include "systemstats.h" #include "i2cstats.h" #include "watchdogstatus.h" @@ -139,11 +139,11 @@ static void systemTask(void *parameters) } #endif - ManualControlCommandData manualControlCommandData; - ManualControlCommandGet(&manualControlCommandData); + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); // Wait until next period - if(manualControlCommandData.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) { + if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 9a4729495..45267f708 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -7384,6 +7384,7 @@ 65C35E5E12EFB2F3004811C2 /* flightplansettings.xml */, 65C35E5F12EFB2F3004811C2 /* flightplanstatus.xml */, 65C35E6012EFB2F3004811C2 /* flighttelemetrystats.xml */, + 65078B09136FCEE600536549 /* flightstatus.xml */, 65C35E6112EFB2F3004811C2 /* gcstelemetrystats.xml */, 65C35E6212EFB2F3004811C2 /* gpsposition.xml */, 65C35E6312EFB2F3004811C2 /* gpssatellites.xml */, @@ -7411,7 +7412,6 @@ 65C35E7712EFB2F3004811C2 /* velocityactual.xml */, 65C35E7812EFB2F3004811C2 /* velocitydesired.xml */, 65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */, - 65078B09136FCEE600536549 /* flightstatus.xml */, ); path = uavobjectdefinition; sourceTree = ""; diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index cab420183..58d6d9330 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -2,7 +2,6 @@ The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control. - From 10e55c25bc5c57f35f0f034584a8ea9192e8d886 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 7 May 2011 15:06:04 -0500 Subject: [PATCH 04/20] OP-475: More changes to get the FlightStatus object to work for Armed and FlightMode --- flight/Modules/Stabilization/stabilization.c | 13 +++++-------- .../plugins/gcscontrol/gcscontrolgadget.cpp | 10 +++++++--- .../gcscontrol/gcscontrolgadgetwidget.cpp | 18 +++++++++++------- .../src/plugins/uavobjects/uavobjects.pro | 2 ++ 4 files changed, 25 insertions(+), 18 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 70b9fa1ae..702020966 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -143,10 +143,6 @@ static void stabilizationTask(void* parameters) continue; } - // Clear alarms - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); - - // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound @@ -160,9 +156,6 @@ static void stabilizationTask(void* parameters) RateDesiredGet(&rateDesired); SystemSettingsGet(&systemSettings); - if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) - return; - #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; @@ -269,7 +262,8 @@ static void stabilizationTask(void* parameters) // Save dT actuatorDesired.UpdateTime = dT * 1000; - + if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) + shouldUpdate = 0; if(shouldUpdate) { @@ -285,6 +279,9 @@ static void stabilizationTask(void* parameters) ZeroPids(); } + + // Clear alarms + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index eda00f86e..9480d789e 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -249,13 +249,17 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) case 1://Armed if (currentCGSControl) { - if(obj->getField("Armed")->getValue().toString().compare("True")==0) + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + + if(obj->getField("Armed")->getValue().toString().compare("Armed")==0) { - obj->getField("Armed")->setValue("False"); + obj->getField("Armed")->setValue("Disarmed"); } else { - obj->getField("Armed")->setValue("True"); + obj->getField("Armed")->setValue("Armed"); } } break; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 4f0ac64e6..287479d8d 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -52,7 +52,8 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) m_gcscontrol->checkBoxGcsControl->setChecked(mdata.flightAccess == UAVObject::ACCESS_READONLY); // Set up the drop down box for the flightmode - m_gcscontrol->comboBoxFlightMode->addItems(obj->getField("FlightMode")->getOptions()); + UAVDataObject* flightStatus = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions()); // Set up slots and signals for joysticks connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double))); @@ -133,18 +134,21 @@ void GCSControlGadgetWidget::toggleArmed(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); if(state) - obj->getField("Armed")->setValue("True"); + obj->getField("Armed")->setValue("Armed"); else - obj->getField("Armed")->setValue("False"); + obj->getField("Armed")->setValue("Disarmed"); obj->updated(); } void GCSControlGadgetWidget::mccChanged(UAVObject * obj) { - m_gcscontrol->checkBoxArmed->setChecked(obj->getField("Armed")->getValue() == "True"); - m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(obj->getField("FlightMode")->getValue().toString())); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject* flightStatus = dynamic_cast( 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"); } /*! @@ -154,7 +158,7 @@ void GCSControlGadgetWidget::selectFlightMode(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); UAVObjectField * field = obj->getField("FlightMode"); field->setValue(field->getOptions()[state]); obj->updated(); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 49cc2d011..e895c74ea 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -66,6 +66,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/ahrsstatus.h \ $$UAVOBJECT_SYNTHETICS/watchdogstatus.h \ $$UAVOBJECT_SYNTHETICS/nedaccel.h \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \ + $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \ @@ -112,4 +113,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \ $$UAVOBJECT_SYNTHETICS/nedaccel.cpp \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \ + $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp From 8e06eb3162bdf7e18f6a86c26bdd0f3086972a7a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 7 May 2011 15:07:14 -0500 Subject: [PATCH 05/20] Get the "IDLE_NO_LOAD" level closer for CC with optimizations on, but it would be great if someone actually calibrated this for me! --- flight/CopterControl/System/inc/pios_config.h | 1 + flight/Modules/System/systemmod.c | 3 +++ 2 files changed, 4 insertions(+) diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 67328c0f8..4362dbca2 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -96,6 +96,7 @@ #define PIOS_STABILIZATION_STACK_SIZE 624 #define PIOS_TELEM_STACK_SIZE 500 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 //#define PIOS_QUATERNION_STABILIZATION #endif /* PIOS_CONFIG_H */ diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index 6c5a72d16..1cc3a6a5b 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -52,9 +52,12 @@ // Private constants #define SYSTEM_UPDATE_PERIOD_MS 1000 #define LED_BLINK_RATE_HZ 5 + +#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c // must be updated if the FreeRTOS or compiler // optimisation options are changed. +#endif #if defined(PIOS_MANUAL_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE From 4ec0263dfccd2de8e4bd0756536c759103fe42ef Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 7 May 2011 15:29:21 -0500 Subject: [PATCH 06/20] OP-476: If AttitudeSettings.ZeroDuringArming is true, then while arming (1 second) it will speed up the estimate of gyro bias. --- flight/Modules/Attitude/attitude.c | 14 ++++++++++++++ shared/uavobjectdefinition/attitudesettings.xml | 1 + 2 files changed, 15 insertions(+) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index d6b52d2fa..599c66b1b 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -53,6 +53,7 @@ #include "attituderaw.h" #include "attitudeactual.h" #include "attitudesettings.h" +#include "flightstatus.h" #include "CoordinateConversions.h" #include "pios_flash_w25x.h" @@ -87,6 +88,7 @@ static int16_t accelbias[3]; static float q[4] = {1,0,0,0}; static float R[3][3]; static int8_t rotate = 0; +static bool zero_during_arming = false; /** * Initialise the module, called on startup @@ -133,15 +135,25 @@ static void AttitudeTask(void *parameters) PIOS_FLASH_DISABLE; PIOS_ADXL345_Init(); + zero_during_arming = false; // Main task loop while (1) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + if(xTaskGetTickCount() < 10000) { // For first 5 seconds use accels to get gyro bias accelKp = 1; // Decrease the rate of gyro learning during init accelKi = .5 / (1 + xTaskGetTickCount() / 5000); yawBiasRate = 0.01 / (1 + xTaskGetTickCount() / 5000); + init = 0; + } + else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + accelKi = .01; + yawBiasRate = 0.1; + init = 0; } else if (init == 0) { settingsUpdatedCb(AttitudeSettingsHandle()); init = 1; @@ -313,6 +325,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { yawBiasRate = attitudeSettings.YawBiasRate; gyroGain = attitudeSettings.GyroGain; + zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; + accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index db067b7e6..085c1a123 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -7,6 +7,7 @@ + From f59e7f34102b68083780d1ee642bd9e4b4e6c644 Mon Sep 17 00:00:00 2001 From: elafargue Date: Sun, 8 May 2011 08:55:20 +0200 Subject: [PATCH 07/20] Update default config to match new UAVObjects and avoid the GCS complaining. --- ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini b/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini index 0b1ef6aca..035791451 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini +++ b/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini @@ -1138,7 +1138,7 @@ LineardialGadget\Accel%20Horizontal%20Z\data\useOpenGLFlag=false LineardialGadget\Accel%20Horizontal%20Z\configInfo\version=0.0.0 LineardialGadget\Accel%20Horizontal%20Z\configInfo\locked=false LineardialGadget\Arm%20Status\data\dFile=%%DATAPATH%%dials/default/arm-status.svg -LineardialGadget\Arm%20Status\data\sourceDataObject=ManualControlCommand +LineardialGadget\Arm%20Status\data\sourceDataObject=FlightStatus LineardialGadget\Arm%20Status\data\sourceObjectField=Armed LineardialGadget\Arm%20Status\data\minValue=0 LineardialGadget\Arm%20Status\data\maxValue=100 @@ -1172,7 +1172,7 @@ LineardialGadget\Flight%20Time\data\useOpenGLFlag=false LineardialGadget\Flight%20Time\configInfo\version=0.0.0 LineardialGadget\Flight%20Time\configInfo\locked=false LineardialGadget\Flight%20mode\data\dFile=%%DATAPATH%%dials/default/flightmode-status.svg -LineardialGadget\Flight%20mode\data\sourceDataObject=ManualControlCommand +LineardialGadget\Flight%20mode\data\sourceDataObject=FlightStatus LineardialGadget\Flight%20mode\data\sourceObjectField=FlightMode LineardialGadget\Flight%20mode\data\minValue=0 LineardialGadget\Flight%20mode\data\maxValue=100 From 2b11fa6ee6484204361dd1201bf19457f8ff7177 Mon Sep 17 00:00:00 2001 From: elafargue Date: Sun, 8 May 2011 09:33:54 +0200 Subject: [PATCH 08/20] Add support for Gyro bias calibration upon arming configuration in the CC Attitude config gadget. --- .../src/plugins/config/ccattitude.ui | 27 ++++++++++++++++++- .../plugins/config/configccattitudewidget.cpp | 13 +++++++-- 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui index 59afdeaf8..7c138ecd8 100644 --- a/ground/openpilotgcs/src/plugins/config/ccattitude.ui +++ b/ground/openpilotgcs/src/plugins/config/ccattitude.ui @@ -7,7 +7,7 @@ 0 0 400 - 307 + 343 @@ -142,6 +142,18 @@ + + + + If enabled, a fast recalibration of gyro zero point will be done +whenever the frame is armed. Do not move the airframe while +arming it in that case! + + + Zero gyro bias upon airframe arming + + + @@ -241,6 +253,19 @@ + + + + Qt::Vertical + + + + 20 + 40 + + + + diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 384e42090..d8a603963 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -53,7 +53,6 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { QMutexLocker locker(&startStop); - UAVDataObject * attitudeRaw = dynamic_cast(obj); ui->zeroBiasProgress->setValue((float) updates / NUM_ACCEL_UPDATES * 100); @@ -87,7 +86,6 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { void ConfigCCAttitudeWidget::timeout() { QMutexLocker locker(&startStop); - UAVObjectManager * objMngr = getObjectManager(); UAVDataObject * obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -108,6 +106,11 @@ void ConfigCCAttitudeWidget::applyAttitudeSettings() { field->setValue(ui->pitchBias->value(),1); field->setValue(ui->yawBias->value(),2); + field = settings->getField("ZeroDuringArming"); + // Handling of boolean values is done through enums on + // uavobjects... + field->setValue((ui->zeroGyroBiasOnArming->isChecked()) ? "TRUE": "FALSE"); + settings->updated(); } @@ -118,6 +121,12 @@ void ConfigCCAttitudeWidget::getCurrentAttitudeSettings() { ui->rollBias->setValue(field->getDouble(0)); ui->pitchBias->setValue(field->getDouble(1)); ui->yawBias->setValue(field->getDouble(2)); + field = settings->getField("ZeroDuringArming"); + // Handling of boolean values is done through enums on + // uavobjects... + bool enabled = (field->getValue().toString() == "FALSE") ? false : true; + ui->zeroGyroBiasOnArming->setChecked(enabled); + } void ConfigCCAttitudeWidget::startAccelCalibration() { From 61007fd0eba46bf69a26aa82ece84379e4ffcb7f Mon Sep 17 00:00:00 2001 From: elafargue Date: Sun, 8 May 2011 09:44:23 +0200 Subject: [PATCH 09/20] Update Arm status dial to reflect changes too. --- artwork/Dials/default/arm-status.svg | 435 +++++++++++++++++- .../openpilotgcs/dials/default/arm-status.svg | 435 +++++++++++++++++- 2 files changed, 852 insertions(+), 18 deletions(-) diff --git a/artwork/Dials/default/arm-status.svg b/artwork/Dials/default/arm-status.svg index 090aa18b5..5913ada22 100644 --- a/artwork/Dials/default/arm-status.svg +++ b/artwork/Dials/default/arm-status.svg @@ -14,7 +14,7 @@ height="80.827866" id="svg10068" version="1.1" - inkscape:version="0.47 r22583" + inkscape:version="0.48.0 r9654" sodipodi:docname="arm-status.svg" inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png" inkscape:export-xdpi="103.61" @@ -828,6 +828,402 @@ inkscape:vp_y="0 : 1000 : 0" inkscape:vp_x="0 : 24 : 1" sodipodi:type="inkscape:persp3d" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml - + Edouard Lafargue @@ -977,7 +1373,7 @@ d="m 53.950159,30.352861 c 0,13.351915 -10.823865,24.17578 -24.175779,24.17578 -13.351915,0 -24.1757796,-10.823865 -24.1757796,-24.17578 0,-13.351914 10.8238646,-24.1757789 24.1757796,-24.1757789 13.351914,0 24.175779,10.8238649 24.175779,24.1757789 z" /> + + + + + diff --git a/ground/openpilotgcs/share/openpilotgcs/dials/default/arm-status.svg b/ground/openpilotgcs/share/openpilotgcs/dials/default/arm-status.svg index 090aa18b5..5913ada22 100644 --- a/ground/openpilotgcs/share/openpilotgcs/dials/default/arm-status.svg +++ b/ground/openpilotgcs/share/openpilotgcs/dials/default/arm-status.svg @@ -14,7 +14,7 @@ height="80.827866" id="svg10068" version="1.1" - inkscape:version="0.47 r22583" + inkscape:version="0.48.0 r9654" sodipodi:docname="arm-status.svg" inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png" inkscape:export-xdpi="103.61" @@ -828,6 +828,402 @@ inkscape:vp_y="0 : 1000 : 0" inkscape:vp_x="0 : 24 : 1" sodipodi:type="inkscape:persp3d" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml - + Edouard Lafargue @@ -977,7 +1373,7 @@ d="m 53.950159,30.352861 c 0,13.351915 -10.823865,24.17578 -24.175779,24.17578 -13.351915,0 -24.1757796,-10.823865 -24.1757796,-24.17578 0,-13.351914 10.8238646,-24.1757789 24.1757796,-24.1757789 13.351914,0 24.175779,10.8238649 24.175779,24.1757789 z" /> + + + + + From 785054a6948fb6556448b0713f61d2ec62e29a6e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 8 May 2011 16:04:26 -0500 Subject: [PATCH 10/20] OP-475: Update HITL for the FlightStatus object --- ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp | 6 +++--- ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp | 1 + ground/openpilotgcs/src/plugins/hitlnew/simulator.h | 2 ++ 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp index f72c78cd6..abfb9f75f 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp @@ -155,17 +155,17 @@ void FGSimulator::processReadyRead() void FGSimulator::transmitUpdate() { ActuatorDesired::DataFields actData; + FlightStatus::DataFields flightStatusData = flightStatus->getData(); ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); - float ailerons = -1; float elevator = -1; float rudder = -1; float throttle = -1; - if(manCtrlData.FlightMode == ManualControlCommand::FLIGHTMODE_MANUAL) + if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) { // Read joystick input - if(manCtrlData.Armed == ManualControlCommand::ARMED_TRUE) + if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) { // Note: Pitch sign is reversed in FG ? ailerons = manCtrlData.Roll; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp index 6d58304b2..c6e2a27e1 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp @@ -125,6 +125,7 @@ void Simulator::onStart() UAVObjectManager* objManager = pm->getObject(); actDesired = ActuatorDesired::GetInstance(objManager); manCtrlCommand = ManualControlCommand::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); posHome = HomeLocation::GetInstance(objManager); velActual = VelocityActual::GetInstance(objManager); posActual = PositionActual::GetInstance(objManager); diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h index 2b3e017a4..3f5f65355 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h @@ -46,6 +46,7 @@ #include "homelocation.h" #include "attituderaw.h" #include "gcstelemetrystats.h" +#include "flightstatus.h" #include "utils/coordinateconversions.h" @@ -170,6 +171,7 @@ protected: ActuatorDesired* actDesired; ManualControlCommand* manCtrlCommand; + FlightStatus* flightStatus; BaroAltitude* altActual; AttitudeActual* attActual; VelocityActual* velActual; From 9dc3dde2c859f0eb376c52c8a627e005c2226ee2 Mon Sep 17 00:00:00 2001 From: elafargue Date: Mon, 9 May 2011 11:19:55 +0200 Subject: [PATCH 11/20] Small UI fix to avoid labels looking ugly on config input panel. --- ground/openpilotgcs/src/plugins/config/input.ui | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 422260fa5..83027fcc0 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -919,9 +919,9 @@ Neutral should be put at the bottom of the slider for the throttle. - 270 + 260 0 - 251 + 291 41 @@ -968,7 +968,7 @@ Neutral should be put at the bottom of the slider for the throttle. - RC Receiver Type: + Receiver Type: Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter From b89909756ca0960261959c41c2f8a909f012597d Mon Sep 17 00:00:00 2001 From: David Carlson Date: Mon, 9 May 2011 02:55:05 -0700 Subject: [PATCH 12/20] Increase accel bias samples to 60. Add debuging output if enabled during calibration. Add check for the possibility of multiple signals arriving before slot is disconnected and causing bias to be calculated incorrectly. --- .../src/plugins/config/configccattitudewidget.cpp | 13 ++++++++++--- .../src/plugins/config/configccattitudewidget.h | 2 +- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 384e42090..9c8190a3b 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -63,7 +63,9 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { x_accum.append(field->getDouble(0)); y_accum.append(field->getDouble(1)); z_accum.append(field->getDouble(2)); - } else { + qDebug("update %d: %f, %f, %f\n",updates,field->getDouble(0),field->getDouble(1),field->getDouble(2)); + } else if ( updates == NUM_ACCEL_UPDATES ) { + updates++; timer.stop(); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -79,9 +81,14 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { field->setDouble(field->getDouble(0) + x_bias,0); field->setDouble(field->getDouble(1) + y_bias,1); field->setDouble(field->getDouble(2) + z_bias,2); + qDebug("New X bias: %f\n", field->getDouble(0)+x_bias); + qDebug("New Y bias: %f\n", field->getDouble(1)+y_bias); + qDebug("New Z bias: %f\n", field->getDouble(2)+z_bias); settings->updated(); ui->status->setText("Calibration done."); - + } else { + // Possible to get here if weird threading stuff happens. Just ignore updates. + qDebug("Unexpected accel update received."); } } @@ -135,7 +142,7 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); // Set up timeout timer - timer.start(2000); + timer.start(10000); connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); // Speed up updates diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index 888fd9754..a70c552ba 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -64,7 +64,7 @@ private: QList x_accum, y_accum, z_accum; - static const int NUM_ACCEL_UPDATES = 10; + static const int NUM_ACCEL_UPDATES = 60; static const float ACCEL_SCALE = 0.004f * 9.81f; }; From c36b6a715baf82693039ff076f9a569311f2ca1f Mon Sep 17 00:00:00 2001 From: elafargue Date: Mon, 9 May 2011 14:24:36 +0200 Subject: [PATCH 13/20] OP-451 Add a tooltip for input configuration channel reversal checkboxes, as requested. --- .../openpilotgcs/src/plugins/config/input.ui | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 83027fcc0..61462ed5f 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -1200,6 +1200,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1226,6 +1231,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1239,6 +1249,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1252,6 +1267,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1265,6 +1285,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1278,6 +1303,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1291,6 +1321,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + @@ -1304,6 +1339,11 @@ p, li { white-space: pre-wrap; } 22 + + Check this to reverse the channel. +(Useful for transmitters without channel +reversal capabilities). + From a97efad8f935b7bd3f14305bfe4d3a149b0cb227 Mon Sep 17 00:00:00 2001 From: elafargue Date: Mon, 9 May 2011 15:19:10 +0200 Subject: [PATCH 14/20] Modify "StoragePath" to use the settings storage directory and not the home directory. Works on Linux. --- ground/openpilotgcs/src/libs/utils/pathutils.cpp | 13 ++++++++----- ground/openpilotgcs/src/libs/utils/pathutils.h | 1 + 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/src/libs/utils/pathutils.cpp b/ground/openpilotgcs/src/libs/utils/pathutils.cpp index a4942becf..9b42dfdf5 100644 --- a/ground/openpilotgcs/src/libs/utils/pathutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathutils.cpp @@ -96,12 +96,16 @@ QString PathUtils::InsertDataPath(QString path) QString PathUtils::GetStoragePath() { // This routine works with "/" as the standard: - // Figure out root: Up one from 'bin' - const QString homeDirPath = QDir::home().canonicalPath(); + // Work out where the settings are stored on the machine + QSettings set(QSettings::IniFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS")); + QFileInfo f(set.fileName()); + QDir dir(f.absoluteDir()); + + const QString homeDirPath = dir.canonicalPath(); QString storagePath = homeDirPath; storagePath += QLatin1Char('/'); - storagePath += QLatin1String("OpenPilot"); - storagePath += QLatin1Char('/'); + // storagePath += QLatin1String("OpenPilot"); + // storagePath += QLatin1Char('/'); return storagePath; } @@ -118,7 +122,6 @@ QString PathUtils::RemoveStoragePath(QString path) return QString("%%STOREPATH%%") + goodPath.right(i); } else return goodPath; - } /** diff --git a/ground/openpilotgcs/src/libs/utils/pathutils.h b/ground/openpilotgcs/src/libs/utils/pathutils.h index b8e459b85..6cd8b36c3 100644 --- a/ground/openpilotgcs/src/libs/utils/pathutils.h +++ b/ground/openpilotgcs/src/libs/utils/pathutils.h @@ -33,6 +33,7 @@ #include "../extensionsystem/pluginmanager.h" #include #include +#include namespace Utils { From 262cc805b0e45117fba9be7bfa0980242cfd81fa Mon Sep 17 00:00:00 2001 From: elafargue Date: Mon, 9 May 2011 16:09:35 +0200 Subject: [PATCH 15/20] Updated opmap lib and opmap plugin to use the Utils::StoragePath correctly. --- ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro | 1 + ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri | 2 ++ .../openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp | 7 ++----- .../openpilotgcs/src/libs/opmapcontrol/src/core/core.pro | 3 +++ .../src/plugins/opmap/opmapgadgetconfiguration.cpp | 5 ----- .../src/plugins/opmap/opmapgadgetoptionspage.cpp | 6 +----- 6 files changed, 9 insertions(+), 15 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro index 6a3ccf724..dee478c28 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro @@ -1,3 +1,4 @@ TEMPLATE = subdirs + SUBDIRS = src \ diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri b/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri index fa9b16603..6907c6f63 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri @@ -1,4 +1,5 @@ DESTDIR = ../build + QT += network QT += sql CONFIG += staticlib @@ -6,3 +7,4 @@ TEMPLATE = lib UI_DIR = uics MOC_DIR = mocs OBJECTS_DIR = objs +INCLUDEPATH +=../../../../libs/ diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp index 8f054dcbb..eb910b56a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp @@ -25,6 +25,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "cache.h" +#include "utils/pathutils.h" #include namespace core { @@ -53,11 +54,7 @@ namespace core { { if(cache.isNull()|cache.isEmpty()) { - QSettings set(QSettings::IniFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS")); - QDir dir(set.fileName()); - QFileInfo f(dir.absolutePath()); - f.dir().absolutePath(); - cache=f.dir().absolutePath()+QDir::separator()+"mapscache"+QDir::separator(); + cache= Utils::PathUtils().GetStoragePath()+"mapscache"+QDir::separator(); setCacheLocation(cache); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro index 9c355e3ca..51387f9f8 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro @@ -1,5 +1,8 @@ include (../common.pri) +include(../../../utils/utils.pri) + + SOURCES += opmaps.cpp \ pureimagecache.cpp \ pureimage.cpp \ diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp index 48f824a3d..25069571f 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp @@ -42,11 +42,6 @@ OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings* m_cacheLocation(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()), m_uavSymbol(QString::fromUtf8(":/uavs/images/mapquad.png")) { - QSettings set(QSettings::IniFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS")); - QDir dir(set.fileName()); - QFileInfo f(dir.absolutePath()); - f.dir().absolutePath(); - m_cacheLocation=f.dir().absolutePath()+QDir::separator() + "mapscache" + QDir::separator(); //if a saved configuration exists load it if(qSettings != 0) { diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp index 816b9fd26..b6f07b9bd 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp @@ -109,11 +109,7 @@ void OPMapGadgetOptionsPage::on_pushButtonCacheDefaults_clicked() m_page->accessModeComboBox->setCurrentIndex(index); m_page->checkBoxUseMemoryCache->setChecked(true); - QSettings set(QSettings::IniFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS")); - QDir dir(set.fileName()); - QFileInfo f(dir.absolutePath()); - f.dir().absolutePath(); - m_page->lineEditCacheLocation->setPath(f.dir().absolutePath()+QDir::separator() + "mapscache" + QDir::separator()); + m_page->lineEditCacheLocation->setPath(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()); } From 822017953e35fd4af656fd8f3ce8bff92c309dad Mon Sep 17 00:00:00 2001 From: elafargue Date: Mon, 9 May 2011 16:48:46 +0200 Subject: [PATCH 16/20] Revert change for opmaplib, causes linking problems on Windows. --- ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri | 1 - .../openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp | 7 +++++-- .../openpilotgcs/src/libs/opmapcontrol/src/core/core.pro | 3 --- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri b/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri index 6907c6f63..076356882 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri @@ -7,4 +7,3 @@ TEMPLATE = lib UI_DIR = uics MOC_DIR = mocs OBJECTS_DIR = objs -INCLUDEPATH +=../../../../libs/ diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp index eb910b56a..8f054dcbb 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp @@ -25,7 +25,6 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "cache.h" -#include "utils/pathutils.h" #include namespace core { @@ -54,7 +53,11 @@ namespace core { { if(cache.isNull()|cache.isEmpty()) { - cache= Utils::PathUtils().GetStoragePath()+"mapscache"+QDir::separator(); + QSettings set(QSettings::IniFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS")); + QDir dir(set.fileName()); + QFileInfo f(dir.absolutePath()); + f.dir().absolutePath(); + cache=f.dir().absolutePath()+QDir::separator()+"mapscache"+QDir::separator(); setCacheLocation(cache); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro index 51387f9f8..9c355e3ca 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro @@ -1,8 +1,5 @@ include (../common.pri) -include(../../../utils/utils.pri) - - SOURCES += opmaps.cpp \ pureimagecache.cpp \ pureimage.cpp \ From 75c57615064dc7f0bdddc9e8fe392691e100613d Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 9 May 2011 19:36:17 +0300 Subject: [PATCH 17/20] Revert "Revert change for opmaplib, causes linking problems on Windows." This reverts commit 822017953e35fd4af656fd8f3ce8bff92c309dad and fixes it to work on Windows too. --- ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri | 1 + .../openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp | 7 ++----- .../src/libs/opmapcontrol/src/mapwidget/mapwidget.pro | 3 +++ ground/openpilotgcs/src/libs/utils/utils.pri | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri b/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri index 076356882..6907c6f63 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/common.pri @@ -7,3 +7,4 @@ TEMPLATE = lib UI_DIR = uics MOC_DIR = mocs OBJECTS_DIR = objs +INCLUDEPATH +=../../../../libs/ diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp index 8f054dcbb..eb910b56a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp @@ -25,6 +25,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "cache.h" +#include "utils/pathutils.h" #include namespace core { @@ -53,11 +54,7 @@ namespace core { { if(cache.isNull()|cache.isEmpty()) { - QSettings set(QSettings::IniFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS")); - QDir dir(set.fileName()); - QFileInfo f(dir.absolutePath()); - f.dir().absolutePath(); - cache=f.dir().absolutePath()+QDir::separator()+"mapscache"+QDir::separator(); + cache= Utils::PathUtils().GetStoragePath()+"mapscache"+QDir::separator(); setCacheLocation(cache); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro index a5aa6082c..496d3b556 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro @@ -3,6 +3,8 @@ TARGET = opmapwidget DEFINES += OPMAPWIDGET_LIBRARY include(../../../../openpilotgcslibrary.pri) +include(../../../utils/utils.pri) + # DESTDIR = ../build SOURCES += mapgraphicitem.cpp \ opmapwidget.cpp \ @@ -15,6 +17,7 @@ SOURCES += mapgraphicitem.cpp \ mapripform.cpp \ mapripper.cpp \ traillineitem.cpp + LIBS += -L../build \ -lcore \ -linternals \ diff --git a/ground/openpilotgcs/src/libs/utils/utils.pri b/ground/openpilotgcs/src/libs/utils/utils.pri index 4e173f2ca..dec80010f 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pri +++ b/ground/openpilotgcs/src/libs/utils/utils.pri @@ -1 +1 @@ -LIBS *= -l$$qtLibraryTarget(Utils) +LIBS += -lcore -l$$qtLibraryTarget(Utils) From 808e3c8ea2f22e4b0e89c8ebfd383d5cf240845b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 9 May 2011 11:37:06 -0500 Subject: [PATCH 18/20] Fix bug I made where guidance didn't look at new flight status object --- flight/Modules/Guidance/guidance.c | 10 +++++----- flight/OpenPilot/UAVObjects.inc | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index 5bdd1f52e..d19ef9311 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -51,7 +51,7 @@ #include "positiondesired.h" // object that will be updated by the module #include "positionactual.h" #include "manualcontrol.h" -#include "manualcontrolcommand.h" +#include "flightstatus.h" #include "nedaccel.h" #include "stabilizationdesired.h" #include "stabilizationsettings.h" @@ -114,7 +114,7 @@ static void guidanceTask(void *parameters) { SystemSettingsData systemSettings; GuidanceSettingsData guidanceSettings; - ManualControlCommandData manualControl; + FlightStatusData flightStatus; portTickType thisTime; portTickType lastUpdateTime; @@ -182,11 +182,11 @@ static void guidanceTask(void *parameters) NedAccelSet(&accelData); - ManualControlCommandGet(&manualControl); + FlightStatusGet(&flightStatus); SystemSettingsGet(&systemSettings); GuidanceSettingsGet(&guidanceSettings); - if ((PARSE_FLIGHT_MODE(manualControl.FlightMode) == FLIGHTMODE_GUIDANCE) && + if ((PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_GUIDANCE) && ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) || @@ -204,7 +204,7 @@ static void guidanceTask(void *parameters) positionHoldLast = 1; } - if( manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD ) + if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) updateVtolDesiredVelocity(); else manualSetDesiredVelocity(); diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index 270fe8b9b..4849367a9 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -67,6 +67,7 @@ UAVOBJSRCFILENAMES += telemetrysettings UAVOBJSRCFILENAMES += velocityactual UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) From 1cae0cd167a6e62b395be56a17999f29b4938aaa Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 9 May 2011 20:09:00 +0300 Subject: [PATCH 19/20] OP-179: final fix (builds on Linux and Windows) --- .../src/libs/opmapcontrol/src/mapwidget/mapwidget.pro | 5 +++-- ground/openpilotgcs/src/libs/utils/utils.pri | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro index 496d3b556..b2098fb78 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro @@ -3,8 +3,6 @@ TARGET = opmapwidget DEFINES += OPMAPWIDGET_LIBRARY include(../../../../openpilotgcslibrary.pri) -include(../../../utils/utils.pri) - # DESTDIR = ../build SOURCES += mapgraphicitem.cpp \ opmapwidget.cpp \ @@ -23,6 +21,9 @@ LIBS += -L../build \ -linternals \ -lcore +# order of linking matters +include(../../../utils/utils.pri) + POST_TARGETDEPS += ../build/libcore.a POST_TARGETDEPS += ../build/libinternals.a diff --git a/ground/openpilotgcs/src/libs/utils/utils.pri b/ground/openpilotgcs/src/libs/utils/utils.pri index dec80010f..572895636 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pri +++ b/ground/openpilotgcs/src/libs/utils/utils.pri @@ -1 +1 @@ -LIBS += -lcore -l$$qtLibraryTarget(Utils) +LIBS += -l$$qtLibraryTarget(Utils) From 94c49ac6d099a655ab8ce4e4e9862b6296302be0 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 9 May 2011 21:05:23 +0300 Subject: [PATCH 20/20] OP-179: fix library dependencies --- ground/openpilotgcs/src/libs/libs.pro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/libs/libs.pro b/ground/openpilotgcs/src/libs/libs.pro index c980b1b1e..af3aaca8c 100644 --- a/ground/openpilotgcs/src/libs/libs.pro +++ b/ground/openpilotgcs/src/libs/libs.pro @@ -6,10 +6,10 @@ SUBDIRS = \ qtconcurrent \ aggregation \ extensionsystem \ + utils \ opmapcontrol \ qwt \ qextserialport \ - utils \ glc_lib\ sdlgamepad \ libqxt