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/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 522a714ac..f499d549a 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -151,6 +151,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/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/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/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/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/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 98996ee1e..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) @@ -54,6 +55,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 +72,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(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); @@ -125,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) @@ -159,18 +164,16 @@ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; - portTickType lastSysTime; - + FlightStatusData flightStatus; 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); - cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; - ManualControlCommandSet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; armState = ARM_STATE_DISARMED; // Main task loop @@ -211,7 +214,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 +230,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 +257,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(&settings, flightMode); + processArm(&cmd, &settings); // Update cmd object ManualControlCommandSet(&cmd); @@ -435,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); @@ -450,7 +301,7 @@ static void manualControlTask(void *parameters) case FLIGHTMODE_GUIDANCE: // TODO: Implement break; - } + } } } @@ -474,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: @@ -517,7 +370,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 +390,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 +430,161 @@ 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 + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + } 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 + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + 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: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + + // only allow arming if it's OK too + if (manualArm && okToArm()) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + 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) + 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; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + 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(ManualControlSettingsData * settings, float flightMode) +{ + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + uint8_t newMode; + // Note here the code is ass + if (flightMode < -FLIGHT_MODE_LIMIT) + newMode = settings->FlightModePosition[0]; + else if (flightMode > FLIGHT_MODE_LIMIT) + newMode = settings->FlightModePosition[2]; + else + newMode = settings->FlightModePosition[1]; + + if(flightStatus.FlightMode != newMode) { + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + } + +} + /** * @brief Determine if the manual input value is within acceptable limits * @returns return TRUE if so, otherwise return FALSE @@ -606,16 +600,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; -// } -// -// -//} /** * @} * @} diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 73f2f39db..702020966 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); @@ -148,13 +149,13 @@ static void stabilizationTask(void* parameters) 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 defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; @@ -261,11 +262,8 @@ static void stabilizationTask(void* parameters) // Save dT actuatorDesired.UpdateTime = dT * 1000; - if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) - { + if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) shouldUpdate = 0; - } - if(shouldUpdate) { @@ -275,14 +273,15 @@ 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); + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } } diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index a3785dac6..1cc3a6a5b 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" @@ -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 @@ -139,11 +142,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/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) ) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 5b209188a..45267f708 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 = ""; }; @@ -7383,6 +7384,7 @@ 65C35E5E12EFB2F3004811C2 /* flightplansettings.xml */, 65C35E5F12EFB2F3004811C2 /* flightplanstatus.xml */, 65C35E6012EFB2F3004811C2 /* flighttelemetrystats.xml */, + 65078B09136FCEE600536549 /* flightstatus.xml */, 65C35E6112EFB2F3004811C2 /* gcstelemetrystats.xml */, 65C35E6212EFB2F3004811C2 /* gpsposition.xml */, 65C35E6312EFB2F3004811C2 /* gpssatellites.xml */, 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" /> + + + + + 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 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/mapwidget/mapwidget.pro b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro index a5aa6082c..b2098fb78 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro @@ -15,11 +15,15 @@ SOURCES += mapgraphicitem.cpp \ mapripform.cpp \ mapripper.cpp \ traillineitem.cpp + LIBS += -L../build \ -lcore \ -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/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 { diff --git a/ground/openpilotgcs/src/libs/utils/utils.pri b/ground/openpilotgcs/src/libs/utils/utils.pri index 4e173f2ca..572895636 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pri +++ b/ground/openpilotgcs/src/libs/utils/utils.pri @@ -1 +1 @@ -LIBS *= -l$$qtLibraryTarget(Utils) +LIBS += -l$$qtLibraryTarget(Utils) 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..c8d278cfc 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); @@ -63,7 +62,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,15 +80,19 @@ 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."); } } 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 +113,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 +128,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() { @@ -135,7 +151,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; }; diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 422260fa5..61462ed5f 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 @@ -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). + diff --git a/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini b/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.ini index 23a84a1d5..37f9d42f5 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 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/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; 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()); } 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 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 @@ + 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..58d6d9330 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -2,15 +2,11 @@ The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control. - - - - - +