From aecc8543a53950657fcd9986481557218b111600 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:09:17 +0200 Subject: [PATCH 001/113] Corrected throttle stick exponentional function in AH mode --- flight/modules/ManualControl/manualcontrol.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 495f1d4a1..17109f170 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -863,10 +863,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 - // then apply an "exp" f(x,k) = (k*x*x*x + x*(256−k)) / 256 - altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 + altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 From 45ed66414f09efbe02c06268417a22588602a92c Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:18:29 +0200 Subject: [PATCH 002/113] Changed AH to work with absolute instead of relative altitude --- flight/modules/AltitudeHold/altitudehold.c | 6 +----- flight/modules/ManualControl/manualcontrol.c | 5 ++++- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index a886de64c..768d9afd0 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -174,10 +174,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) error = 0; velocity = 0; running = true; - - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - starting_altitude = altHold.Altitude; } else if (!altitudeHoldFlightMode) { running = false; lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); @@ -364,7 +360,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } // Compute the altitude error - error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude; + error = altitudeHoldDesired.Altitude - altHold.Altitude; // Compute integral off altitude error throttleIntegral += error * altitudeHoldSettings.Ki * dT; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 17109f170..c1efb4ebd 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,6 +38,7 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" +#include "altholdsmoothed.h #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" @@ -859,7 +860,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) PositionStateDownGet(¤tDown); if (changed) { // After not being in this mode for a while init at current height - altitudeHoldDesiredData.Altitude = 0; + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Altitude = altHold.Altitude; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 From 723f308ac72ee0473e78e4894803b2ed01d562d2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:26:28 +0200 Subject: [PATCH 003/113] Removed obsolete (unused) code --- flight/modules/AltitudeHold/altitudehold.c | 18 +++++++----------- flight/modules/ManualControl/manualcontrol.c | 2 -- 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 768d9afd0..e3d32ce92 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -238,17 +238,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) x[0] = baro.Altitude; // rotate avg accels into earth frame and store it - if (1) { - float q[4], Rbe[3][3]; - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - } else { - x[1] = -accelState.z + 9.81f; - } + float q[4], Rbe[3][3]; + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); + x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; timeval = PIOS_DELAY_GetRaw(); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index c1efb4ebd..8e7e99aa4 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -856,8 +856,6 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - float currentDown; - PositionStateDownGet(¤tDown); if (changed) { // After not being in this mode for a while init at current height AltHoldSmoothedData altHold; From 73aa4df431f2aee303ae5c2477ecc1cd84fdb79a Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:44:48 +0200 Subject: [PATCH 004/113] Missing " --- flight/modules/ManualControl/manualcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 8e7e99aa4..96d468b7f 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,7 +38,7 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" -#include "altholdsmoothed.h +#include "altholdsmoothed.h" #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" From 1da68cda9af3f8b36a2b9fe67a55c2bd55e58b16 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:45:40 +0200 Subject: [PATCH 005/113] Scale up desired throttle in AH mode to compensate for roll/pitch --- flight/modules/AltitudeHold/altitudehold.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e3d32ce92..08aa13116 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -125,6 +125,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) { AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; + float q[4], Rbe[3][3]; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; @@ -238,7 +239,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) x[0] = baro.Altitude; // rotate avg accels into earth frame and store it - float q[4], Rbe[3][3]; q[0] = attitudeState.q1; q[1] = attitudeState.q2; q[2] = attitudeState.q3; @@ -373,6 +373,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if (!enterFailSafe) { stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling + float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; + stabilizationDesired.Throttle /= throttlescale; + if (stabilizationDesired.Throttle > 1) { throttleIntegral -= (stabilizationDesired.Throttle - 1); stabilizationDesired.Throttle = 1; From 513d509865d33268ac7f1378c591923f28468b8f Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 15:34:29 +0200 Subject: [PATCH 006/113] Initialize altitude before switching to "running". Not doing this could cause glitches during the very first switch to AH. --- flight/modules/AltitudeHold/altitudehold.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 08aa13116..31bbd4044 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -125,6 +125,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) { AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; + AltHoldSmoothedData altHold; float q[4], Rbe[3][3]; portTickType thisTime, lastUpdateTime; @@ -174,6 +175,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) switchThrottle = throttleIntegral; error = 0; velocity = 0; + altitudeHoldDesired.Altitude = altHold.Altitude; running = true; } else if (!altitudeHoldFlightMode) { running = false; @@ -334,7 +336,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f); } - AltHoldSmoothedData altHold; AltHoldSmoothedGet(&altHold); altHold.Altitude = z[0]; altHold.Velocity = z[1]; From 23b5b374803fbd5031fec967f9eb5a30bcc43582 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Mon, 1 Jul 2013 10:13:27 +0200 Subject: [PATCH 007/113] Scale down initial throttle when AH is engaged based on roll/pitch angle. This improves AH behaviour when switching to AH while being in fast forward flight. --- flight/modules/AltitudeHold/altitudehold.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 31bbd4044..91ff452bf 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -173,6 +173,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&throttleIntegral); switchThrottle = throttleIntegral; + throttleIntegral *= Rbe[2][2]; // rotate into earth frame + if (throttleIntegral > 1) { + throttleIntegral = 1; + } else if (throttleIntegral < 0) { + throttleIntegral = 0; + } error = 0; velocity = 0; altitudeHoldDesired.Altitude = altHold.Altitude; From 133ad414f81e303cdd0ca9e6497456dee89cf889 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 14 Jul 2013 07:46:42 +0000 Subject: [PATCH 008/113] OP-1022: Added a velocity loop in AH module, used when throttle is not in central position. --- flight/modules/AltitudeHold/altitudehold.c | 24 ++++++++++++------- flight/modules/ManualControl/manualcontrol.c | 22 ++++++++--------- .../altitudeholddesired.xml | 1 + .../altitudeholdsettings.xml | 5 ++-- 4 files changed, 30 insertions(+), 22 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 91ff452bf..45c73c611 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -114,6 +114,8 @@ float decay; float velocity_decay; bool running = false; float error; +float velError; + float switchThrottle; float smoothed_altitude; float starting_altitude; @@ -364,9 +366,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Compute the altitude error error = altitudeHoldDesired.Altitude - altHold.Altitude; + velError = altitudeHoldDesired.Velocity - altHold.Velocity; - // Compute integral off altitude error - throttleIntegral += error * altitudeHoldSettings.Ki * dT; + if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + // Compute integral off altitude error + throttleIntegral += error * altitudeHoldSettings.Ki * dT; + } // Only update stabilizationDesired less frequently if ((thisTime - lastUpdateTime) < 20) { @@ -378,12 +383,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; - // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling - float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; - stabilizationDesired.Throttle /= throttlescale; - + if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - + altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling + float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; + stabilizationDesired.Throttle /= throttlescale; + } else { + stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral; + } if (stabilizationDesired.Throttle > 1) { throttleIntegral -= (stabilizationDesired.Throttle - 1); stabilizationDesired.Throttle = 1; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 96d468b7f..4a1fc7127 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -827,10 +827,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) uint8_t throttleExp; static uint8_t flightMode; - static portTickType lastSysTimeAH; static bool zeroed = false; - portTickType thisSysTime; - float dT; FlightStatusFlightModeGet(&flightMode); @@ -848,29 +845,30 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); - lastSysTimeAH = thisSysTime; - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; if (changed) { // After not being in this mode for a while init at current height - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - altitudeHoldDesiredData.Altitude = altHold.Altitude; + + AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Velocity = 0.0f; + zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate * dT; + altitudeHoldDesiredData.Velocity = ((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate * dT; + altitudeHoldDesiredData.Velocity = -((throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 + if(fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { + AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Velocity = 0.0f; + } zeroed = true; } diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index cf056cfa0..bae414384 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,6 +1,7 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through + diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 3a2cf6149..40fdf30be 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,9 +1,10 @@ Settings for the @ref AltitudeHold module - + + - + From a57f8913ba3cb9f9586bb3dd707bbe80a4be6c8d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 14 Jul 2013 20:20:22 +0000 Subject: [PATCH 009/113] OP-1022: replace old KF in alitude hold with status from filters --- flight/modules/AltitudeHold/altitudehold.c | 191 +++---------------- flight/modules/ManualControl/manualcontrol.c | 10 +- 2 files changed, 35 insertions(+), 166 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 45c73c611..59e5670bd 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -58,6 +58,8 @@ #include #include #include +#include +#include // Private constants #define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 @@ -107,18 +109,15 @@ int32_t AltitudeHoldInitialize() } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); -float tau; float throttleIntegral; +float switchThrottle; float velocity; -float decay; -float velocity_decay; bool running = false; float error; float velError; +uint32_t timeval; -float switchThrottle; -float smoothed_altitude; -float starting_altitude; +bool posUpdated; /** * Module thread, should not return. @@ -128,11 +127,17 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltHoldSmoothedData altHold; + AttitudeStateData attitudeState; + AccelStateData accelState; + VelocityStateData velocityData; + float dT; float q[4], Rbe[3][3]; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; + timeval = 0; + lastUpdateTime = 0; // Force update of the settings SettingsUpdatedCb(&ev); // Failsafe handling @@ -140,11 +145,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - BaroSensorConnectQueue(queue); - FlightStatusConnectQueue(queue); AccelStateConnectQueue(queue); + PositionStateConnectQueue(queue); + FlightStatusConnectQueue(queue); + VelocityStateConnectQueue(queue); bool altitudeHoldFlightMode = false; - BaroSensorAltitudeGet(&smoothed_altitude); running = false; enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; @@ -153,7 +158,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // initialize enable flag altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; // Main task loop - bool baro_updated = false; while (1) { enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe @@ -164,8 +168,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Todo: Add alarm if it should be running continue; - } else if (ev.obj == BaroSensorHandle()) { - baro_updated = true; + } else if (ev.obj == PositionStateHandle()) { + posUpdated = true; init = (init == WAITING_BARO) ? WAITIING_INIT : init; } else if (ev.obj == FlightStatusHandle()) { @@ -190,168 +194,30 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } } else if (ev.obj == AccelStateHandle()) { - static uint32_t timeval; + dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; + timeval = PIOS_DELAY_GetRaw(); - static float z[4] = { 0, 0, 0, 0 }; - float z_new[4]; - float P[4][4], K[4][2], x[2]; - float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f }; - static float V[4][4] = { - { 10.0f, 0.0f, 0.0f, 0.0f }, - { 0.0f, 100.0f, 0.0f, 0.0f }, - { 0.0f, 0.0f, 100.0f, 0.0f }, - { 0.0f, 0.0f, 0.0f, 1000.0f } - }; - static uint32_t accel_downsample_count = 0; - static float accels_accum[3] = { 0.0f, 0.0f, 0.0f }; - float dT; - static float S[2] = { 1.0f, 10.0f }; + AltHoldSmoothedGet(&altHold); - /* Somehow this always assigns to zero. Compiler bug? Race condition? */ - S[0] = altitudeHoldSettings.PressureNoise; - S[1] = altitudeHoldSettings.AccelNoise; - G[2] = altitudeHoldSettings.AccelDrift; - - AccelStateData accelState; - AccelStateGet(&accelState); - AttitudeStateData attitudeState; AttitudeStateGet(&attitudeState); - BaroSensorData baro; - BaroSensorGet(&baro); - /* Downsample accels to stop this calculation consuming too much CPU */ - accels_accum[0] += accelState.x; - accels_accum[1] += accelState.y; - accels_accum[2] += accelState.z; - accel_downsample_count++; - - if (accel_downsample_count < ACCEL_DOWNSAMPLE) { - continue; - } - - accel_downsample_count = 0; - accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE; - accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE; - accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE; - accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; - - thisTime = xTaskGetTickCount(); - - if (init == WAITIING_INIT) { - z[0] = baro.Altitude; - z[1] = 0; - z[2] = accelState.z; - z[3] = 0; - init = INITED; - } else if (init == WAITING_BARO) { - continue; - } - - x[0] = baro.Altitude; - // rotate avg accels into earth frame and store it q[0] = attitudeState.q1; q[1] = attitudeState.q2; q[2] = attitudeState.q3; q[3] = attitudeState.q4; Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; - timeval = PIOS_DELAY_GetRaw(); + AccelStateGet(&accelState); + altHold.Accel = 0.95f * altHold.Accel + 0.05f * (Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0]; - P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1]; - P[0][2] = V[0][2] + dT * V[1][2]; - P[0][3] = V[0][3] + dT * V[1][3]; - P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0]; - P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1]; - P[1][2] = V[1][2] + dT * V[2][2]; - P[1][3] = V[1][3] + dT * V[2][3]; - P[2][0] = V[2][0] + dT * V[2][1]; - P[2][1] = V[2][1] + dT * V[2][2]; - P[2][2] = V[2][2] + G[2]; - P[2][3] = V[2][3]; - P[3][0] = V[3][0] + dT * V[3][1]; - P[3][1] = V[3][1] + dT * V[3][2]; - P[3][2] = V[3][2]; - P[3][3] = V[3][3] + G[3]; + VelocityStateGet(&velocityData); - if (baro_updated) { - K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f; - K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + altHold.Velocity = 0.95f * altHold.Velocity + 0.05f * (Rbe[0][2] * velocityData.East + Rbe[1][2] * velocityData.North + Rbe[2][2] * velocityData.Down); - z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0]; - z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1]; - z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2]; - z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3]; + PositionStateDownGet(&altHold.Altitude); - memcpy(z, z_new, sizeof(z_new)); - - V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f); - V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f); - V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f); - V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f); - V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0]; - V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2]; - V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2]; - V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3]; - V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f); - V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f); - V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f); - V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f); - V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f); - V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f); - V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f); - V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f); - - - baro_updated = false; - } else { - K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - - - z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0]; - z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1]; - z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2]; - z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3]; - - memcpy(z, z_new, sizeof(z_new)); - - V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0]; - V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2]; - V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2]; - V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3]; - V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0]; - V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2]; - V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2]; - V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3]; - V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f); - V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f); - V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f); - V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f); - V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f); - V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f); - V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f); - V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f); - } - - AltHoldSmoothedGet(&altHold); - altHold.Altitude = z[0]; - altHold.Velocity = z[1]; - altHold.Accel = z[2]; AltHoldSmoothedSet(&altHold); - AltHoldSmoothedGet(&altHold); - // Verify that we are in altitude hold mode uint8_t armed; FlightStatusArmedGet(&armed); @@ -372,7 +238,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Compute integral off altitude error throttleIntegral += error * altitudeHoldSettings.Ki * dT; } - + thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently if ((thisTime - lastUpdateTime) < 20) { continue; @@ -384,14 +250,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + stabilizationDesired.Throttle = - error * altitudeHoldSettings.Kp + throttleIntegral + + altHold.Velocity * altitudeHoldSettings.Kd;// - altHold.Accel * altitudeHoldSettings.Ka; // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; stabilizationDesired.Throttle /= throttlescale; } else { - stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral; + stabilizationDesired.Throttle = -velError * altitudeHoldSettings.Kv + throttleIntegral; } + if (stabilizationDesired.Throttle > 1) { throttleIntegral -= (stabilizationDesired.Throttle - 1); stabilizationDesired.Throttle = 1; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 4a1fc7127..2f505bb16 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -844,6 +844,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); + AltHoldSmoothedData altHoldSmoothed; + AltHoldSmoothedGet(&altHoldSmoothed); altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; @@ -852,21 +854,21 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height - AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; altitudeHoldDesiredData.Velocity = 0.0f; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = ((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.Velocity = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -((throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.Velocity = ((throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 if(fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { - AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; altitudeHoldDesiredData.Velocity = 0.0f; } zeroed = true; From e975e4d9b789015e3933509a08f6debe9cb70244 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 15 Jul 2013 20:33:08 +0000 Subject: [PATCH 010/113] OP-1022: replace old KF in alitude hold with status from filters --- flight/modules/AltitudeHold/altitudehold.c | 56 +++++++++++-------- flight/modules/ManualControl/manualcontrol.c | 4 +- .../altitudeholdsettings.xml | 5 +- 3 files changed, 38 insertions(+), 27 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 59e5670bd..c96797129 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -64,7 +64,7 @@ #define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 4 +#define ACCEL_DOWNSAMPLE 10 #define TIMEOUT_TRESHOLD 200000 // Private types @@ -112,11 +112,12 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); float throttleIntegral; float switchThrottle; float velocity; +float accelAlpha; +float velAlpha; bool running = false; float error; float velError; uint32_t timeval; - bool posUpdated; /** @@ -128,14 +129,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) StabilizationDesiredData stabilizationDesired; AltHoldSmoothedData altHold; AttitudeStateData attitudeState; - AccelStateData accelState; VelocityStateData velocityData; float dT; float q[4], Rbe[3][3]; - + float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; - + lastVertVelocity = 0; timeval = 0; lastUpdateTime = 0; // Force update of the settings @@ -145,7 +145,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - AccelStateConnectQueue(queue); PositionStateConnectQueue(queue); FlightStatusConnectQueue(queue); VelocityStateConnectQueue(queue); @@ -176,6 +175,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) FlightStatusFlightModeGet(&flightMode); altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; if (altitudeHoldFlightMode && !running) { + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&throttleIntegral); switchThrottle = throttleIntegral; @@ -193,28 +198,24 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) running = false; lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } - } else if (ev.obj == AccelStateHandle()) { + } else if (ev.obj == VelocityStateHandle()) { + dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; timeval = PIOS_DELAY_GetRaw(); AltHoldSmoothedGet(&altHold); - AttitudeStateGet(&attitudeState); - - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - - AccelStateGet(&accelState); - altHold.Accel = 0.95f * altHold.Accel + 0.05f * (Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - VelocityStateGet(&velocityData); - altHold.Velocity = 0.95f * altHold.Velocity + 0.05f * (Rbe[0][2] * velocityData.East + Rbe[1][2] * velocityData.North + Rbe[2][2] * velocityData.Down); + altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); + float vertAccel = (velocityData.Down - lastVertVelocity)/dT; + + lastVertVelocity = velocityData.Down; + + altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); PositionStateDownGet(&altHold.Altitude); + altHold.Altitude = -altHold.Altitude; AltHoldSmoothedSet(&altHold); @@ -250,13 +251,22 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - stabilizationDesired.Throttle = - error * altitudeHoldSettings.Kp + throttleIntegral + - altHold.Velocity * altitudeHoldSettings.Kd;// - altHold.Accel * altitudeHoldSettings.Ka; + stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + + throttleIntegral + - altHold.Velocity * altitudeHoldSettings.Kd + - altHold.Accel * altitudeHoldSettings.Ka; + // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; stabilizationDesired.Throttle /= throttlescale; } else { - stabilizationDesired.Throttle = -velError * altitudeHoldSettings.Kv + throttleIntegral; + stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral; } if (stabilizationDesired.Throttle > 1) { @@ -289,4 +299,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); + accelAlpha = expf(-(1000.0f/666.0f * ACCEL_DOWNSAMPLE)/ altitudeHoldSettings.AccelTau); + velAlpha = expf(-(1000.0f/666.0f * ACCEL_DOWNSAMPLE)/ altitudeHoldSettings.VelocityTau); } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 2f505bb16..3f6107f7b 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -861,9 +861,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.Velocity = ((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = ((throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.Velocity = -((throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 40fdf30be..ca190ba56 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -6,9 +6,8 @@ - - - + + From 5b4d46819e7590af402d7a7245cdda01b60d8d32 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 11:58:20 +0000 Subject: [PATCH 011/113] OP-1022 Fix problem with EventSystem due to too high update rate --- flight/modules/AltitudeHold/altitudehold.c | 14 ++++++-------- shared/uavobjectdefinition/altholdsmoothed.xml | 6 ++++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index c96797129..fb86e5443 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -135,6 +135,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; + dT = 0; lastVertVelocity = 0; timeval = 0; lastUpdateTime = 0; @@ -145,7 +146,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - PositionStateConnectQueue(queue); + //PositionStateConnectQueue(queue); FlightStatusConnectQueue(queue); VelocityStateConnectQueue(queue); bool altitudeHoldFlightMode = false; @@ -167,10 +168,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Todo: Add alarm if it should be running continue; - } else if (ev.obj == PositionStateHandle()) { - posUpdated = true; - - init = (init == WAITING_BARO) ? WAITIING_INIT : init; } else if (ev.obj == FlightStatusHandle()) { FlightStatusFlightModeGet(&flightMode); altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; @@ -199,8 +196,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } } else if (ev.obj == VelocityStateHandle()) { - - dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; + init = (init == WAITING_BARO) ? WAITIING_INIT : init; + dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; timeval = PIOS_DELAY_GetRaw(); AltHoldSmoothedGet(&altHold); @@ -213,6 +210,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastVertVelocity = velocityData.Down; altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); + altHold.StateUpdateInterval = dT; PositionStateDownGet(&altHold.Altitude); altHold.Altitude = -altHold.Altitude; @@ -244,7 +242,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if ((thisTime - lastUpdateTime) < 20) { continue; } - + altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; lastUpdateTime = thisTime; // Instead of explicit limit on integral you output limit feedback diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml index b32e977c2..b990228b0 100644 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ b/shared/uavobjectdefinition/altholdsmoothed.xml @@ -2,8 +2,10 @@ The output on the kalman filter on altitude. - - + + + + From 0203e2c6e290c5b3f1625fb9ced75bd6dad0c133 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 20:01:16 +0000 Subject: [PATCH 012/113] OP-1022 Adding a proportional term on the square of the altitude error. --- flight/modules/AltitudeHold/altitudehold.c | 1 + flight/modules/ManualControl/manualcontrol.c | 2 +- .../altitudeholdsettings.xml | 19 ++++++++++--------- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index fb86e5443..85cfaff6b 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -250,6 +250,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if (!enterFailSafe) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + + error * fabsf(error) * altitudeHoldSettings.Kp2 + throttleIntegral - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 3f6107f7b..70730c599 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -813,7 +813,7 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * */ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) { - const float DEADBAND = 0.25f; + const float DEADBAND = 0.20f; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index ca190ba56..f609dc81d 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,15 +1,16 @@ Settings for the @ref AltitudeHold module - - - - - - - - - + + + + + + + + + + From ed233efde20554bccfd105ee38b9d423b42d6184 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 20:02:03 +0000 Subject: [PATCH 013/113] OP-1022 Uncrustify --- flight/modules/AltitudeHold/altitudehold.c | 25 ++++++++++---------- flight/modules/ManualControl/manualcontrol.c | 2 +- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 85cfaff6b..1cb27fc82 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -135,10 +135,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; + dT = 0; lastVertVelocity = 0; timeval = 0; - lastUpdateTime = 0; + lastUpdateTime = 0; // Force update of the settings SettingsUpdatedCb(&ev); // Failsafe handling @@ -146,7 +147,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - //PositionStateConnectQueue(queue); + // PositionStateConnectQueue(queue); FlightStatusConnectQueue(queue); VelocityStateConnectQueue(queue); bool altitudeHoldFlightMode = false; @@ -180,7 +181,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) Quaternion2R(q, Rbe); // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&throttleIntegral); - switchThrottle = throttleIntegral; + switchThrottle = throttleIntegral; throttleIntegral *= Rbe[2][2]; // rotate into earth frame if (throttleIntegral > 1) { throttleIntegral = 1; @@ -196,8 +197,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } } else if (ev.obj == VelocityStateHandle()) { - init = (init == WAITING_BARO) ? WAITIING_INIT : init; - dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; + init = (init == WAITING_BARO) ? WAITIING_INIT : init; + dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; timeval = PIOS_DELAY_GetRaw(); AltHoldSmoothedGet(&altHold); @@ -205,11 +206,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) VelocityStateGet(&velocityData); altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); - float vertAccel = (velocityData.Down - lastVertVelocity)/dT; + float vertAccel = (velocityData.Down - lastVertVelocity) / dT; lastVertVelocity = velocityData.Down; - altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); + altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); altHold.StateUpdateInterval = dT; PositionStateDownGet(&altHold.Altitude); @@ -230,10 +231,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } // Compute the altitude error - error = altitudeHoldDesired.Altitude - altHold.Altitude; + error = altitudeHoldDesired.Altitude - altHold.Altitude; velError = altitudeHoldDesired.Velocity - altHold.Velocity; - if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { // Compute integral off altitude error throttleIntegral += error * altitudeHoldSettings.Ki * dT; } @@ -248,7 +249,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { - if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + error * fabsf(error) * altitudeHoldSettings.Kp2 + throttleIntegral @@ -298,6 +299,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - accelAlpha = expf(-(1000.0f/666.0f * ACCEL_DOWNSAMPLE)/ altitudeHoldSettings.AccelTau); - velAlpha = expf(-(1000.0f/666.0f * ACCEL_DOWNSAMPLE)/ altitudeHoldSettings.VelocityTau); + accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau); + velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 70730c599..b402bde4f 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -867,7 +867,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 - if(fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { + if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; altitudeHoldDesiredData.Velocity = 0.0f; } From 7708aab3133e67e1e0a187d6ee0ece71bdfab1ea Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 11 Jul 2013 18:17:00 +0200 Subject: [PATCH 014/113] Added vertical velocity as a control loop input to improve dynamics during commanded ascend/decent. Conflicts: flight/modules/AltitudeHold/altitudehold.c flight/modules/ManualControl/manualcontrol.c shared/uavobjectdefinition/altitudeholdsettings.xml --- flight/modules/AltitudeHold/altitudehold.c | 78 +++++++++---------- flight/modules/ManualControl/manualcontrol.c | 27 ++++--- .../boards/revolution/firmware/pios_board.c | 2 +- .../altitudeholddesired.xml | 2 +- .../altitudeholdsettings.xml | 3 +- 5 files changed, 57 insertions(+), 55 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 1cb27fc82..fa65d8fa0 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -64,7 +64,7 @@ #define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 10 +#define ACCEL_DOWNSAMPLE 4 #define TIMEOUT_TRESHOLD 200000 // Private types @@ -109,8 +109,11 @@ int32_t AltitudeHoldInitialize() } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); -float throttleIntegral; -float switchThrottle; +float tau; +float altitudeIntegral; +float velocityIntegral; +float decay; +float velocity_decay; float velocity; float accelAlpha; float velAlpha; @@ -131,7 +134,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) AttitudeStateData attitudeState; VelocityStateData velocityData; float dT; - float q[4], Rbe[3][3]; + float q[4], Rbe[3][3], fblimit = 0; float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; @@ -164,7 +167,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) { if (!running) { - throttleIntegral = 0; + altitudeIntegral = 0; } // Todo: Add alarm if it should be running @@ -180,17 +183,19 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) q[3] = attitudeState.q4; Quaternion2R(q, Rbe); // Copy the current throttle as a starting point for integral - StabilizationDesiredThrottleGet(&throttleIntegral); - switchThrottle = throttleIntegral; - throttleIntegral *= Rbe[2][2]; // rotate into earth frame - if (throttleIntegral > 1) { - throttleIntegral = 1; - } else if (throttleIntegral < 0) { - throttleIntegral = 0; + float initThrottle; + StabilizationDesiredThrottleGet(&initThrottle); + initThrottle *= Rbe[2][2]; // rotate into earth frame + if (initThrottle > 1) { + initThrottle = 1; + } else if (initThrottle < 0) { + initThrottle = 0; } error = 0; - velocity = 0; + altitudeHoldDesired.Velocity = 0; altitudeHoldDesired.Altitude = altHold.Altitude; + altitudeIntegral = altHold.Altitude * altitudeHoldSettings.Kp + initThrottle; + velocityIntegral = 0; running = true; } else if (!altitudeHoldFlightMode) { running = false; @@ -230,14 +235,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) continue; } - // Compute the altitude error - error = altitudeHoldDesired.Altitude - altHold.Altitude; - velError = altitudeHoldDesired.Velocity - altHold.Velocity; + // Compute altitude and velocity integral + altitudeIntegral += (altitudeHoldDesired.Altitude - altHold.Altitude - fblimit) * altitudeHoldSettings.AltitudeKi * dT; + velocityIntegral += (altitudeHoldDesired.Velocity - altHold.Velocity - fblimit) * altitudeHoldSettings.VelocityKi * dT; - if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - // Compute integral off altitude error - throttleIntegral += error * altitudeHoldSettings.Ki * dT; - } thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently if ((thisTime - lastUpdateTime) < 20) { @@ -249,31 +250,28 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { - if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp - + error * fabsf(error) * altitudeHoldSettings.Kp2 - + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - - altHold.Accel * altitudeHoldSettings.Ka; - // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; - stabilizationDesired.Throttle /= throttlescale; - } else { - stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral; - } + stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral + + error * fabsf(error) * altitudeHoldSettings.Kp2 + - altHold.Altitude * altitudeHoldSettings.Kp + - altHold.Velocity * altitudeHoldSettings.Kd + - altHold.Accel * altitudeHoldSettings.Ka; + // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); + float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; + stabilizationDesired.Throttle /= throttlescale; + fblimit = 0; if (stabilizationDesired.Throttle > 1) { - throttleIntegral -= (stabilizationDesired.Throttle - 1); + fblimit = stabilizationDesired.Throttle - 1; stabilizationDesired.Throttle = 1; } else if (stabilizationDesired.Throttle < 0) { - throttleIntegral -= stabilizationDesired.Throttle; + fblimit = stabilizationDesired.Throttle; stabilizationDesired.Throttle = 0; } } else { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b402bde4f..58704e0fc 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -827,7 +827,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) uint8_t throttleExp; static uint8_t flightMode; + static portTickType lastSysTimeAH; static bool zeroed = false; + portTickType thisSysTime; + float dT; FlightStatusFlightModeGet(&flightMode); @@ -844,8 +847,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - AltHoldSmoothedData altHoldSmoothed; - AltHoldSmoothedGet(&altHoldSmoothed); + + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); + lastSysTimeAH = thisSysTime; altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; @@ -853,24 +858,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height - - altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; - altitudeHoldDesiredData.Velocity = 0.0f; - + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Altitude = altHold.Altitude; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = ((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -((throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 - if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { - altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; - altitudeHoldDesiredData.Velocity = 0.0f; - } + altitudeHoldDesiredData.Velocity = 0; zeroed = true; } diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index ffc05ce50..f630892b5 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_4096, }; #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index bae414384..e91f5a0c6 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,8 +1,8 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through - + diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index f609dc81d..1b2b2c26e 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -4,7 +4,8 @@ - + + From 45cebda628ae1f334769f68763dd43d54d70805f Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 16 Jul 2013 14:08:59 +0200 Subject: [PATCH 015/113] Add a configurable lowpass filter to smooth throttle commands in AltitudeHold mode. Conflicts: flight/modules/AltitudeHold/altitudehold.c shared/uavobjectdefinition/altitudeholdsettings.xml --- flight/modules/AltitudeHold/altitudehold.c | 20 ++++++++++++++++++- .../altitudeholdsettings.xml | 1 + 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index fa65d8fa0..fd6f5ae68 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -66,12 +66,15 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define ACCEL_DOWNSAMPLE 4 #define TIMEOUT_TRESHOLD 200000 +#define DESIRED_UPDATE_RATE_MS 20 // milliseconds // Private types // Private variables static xTaskHandle altitudeHoldTaskHandle; static xQueueHandle queue; static AltitudeHoldSettingsData altitudeHoldSettings; +static float throttleAlpha = 1.0f; +static float throttle_old = 0.0f; // Private functions static void altitudeHoldTask(void *parameters); @@ -119,6 +122,9 @@ float accelAlpha; float velAlpha; bool running = false; float error; +float switchThrottle; +float smoothed_altitude; +float starting_altitude; float velError; uint32_t timeval; bool posUpdated; @@ -241,7 +247,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently - if ((thisTime - lastUpdateTime) < 20) { + if ((thisTime - lastUpdateTime)*1000/configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { continue; } altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; @@ -265,6 +271,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) Quaternion2R(q, Rbe); float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; stabilizationDesired.Throttle /= throttlescale; + stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); + throttle_old = stabilizationDesired.Throttle; fblimit = 0; if (stabilizationDesired.Throttle > 1) { @@ -299,4 +307,14 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) AltitudeHoldSettingsGet(&altitudeHoldSettings); accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau); velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); + + // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency) + if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f/DESIRED_UPDATE_RATE_MS) + { + throttleAlpha = (float)DESIRED_UPDATE_RATE_MS/((float)DESIRED_UPDATE_RATE_MS + 1000.0f/(2.0f*M_PI_F*altitudeHoldSettings.ThrottleFilterCutoff)); + } + else + { + throttleAlpha = 1.0f; + } } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 1b2b2c26e..76536c0c8 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -10,6 +10,7 @@ + From 7138faf16759772ab08524ff3e67809ab8d3cff2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 16 Jul 2013 14:38:20 +0200 Subject: [PATCH 016/113] Adjusted default settings for altitude hold. Conflicts: shared/uavobjectdefinition/altitudeholdsettings.xml --- .../altitudeholdsettings.xml | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 76536c0c8..bc234931c 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,18 +1,17 @@ Settings for the @ref AltitudeHold module - - - - - - - - - - - - + + + + + + + + + + + From 8c70e64544cdc7279befda24d64b464026eceab9 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 17 Jul 2013 08:37:56 +0000 Subject: [PATCH 017/113] OP-1022 reset velocity when changing flight mode --- flight/modules/ManualControl/manualcontrol.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 58704e0fc..b888c0476 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -860,15 +860,16 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) // After not being in this mode for a while init at current height AltHoldSmoothedData altHold; AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Altitude = altHold.Altitude; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height From 005a68826d78fca58ed926dd67112e9a254153c0 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 17 Jul 2013 08:38:44 +0000 Subject: [PATCH 018/113] OP-1022 Converted to a plain Altitude PID + Velocity PI implementation --- flight/modules/AltitudeHold/altitudehold.c | 72 ++++++++++--------- .../altitudeholdsettings.xml | 8 +-- 2 files changed, 42 insertions(+), 38 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index fd6f5ae68..7e5618165 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -61,11 +61,11 @@ #include #include // Private constants -#define MAX_QUEUE_SIZE 2 -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 4 -#define TIMEOUT_TRESHOLD 200000 +#define MAX_QUEUE_SIZE 2 +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define ACCEL_DOWNSAMPLE 4 +#define TIMEOUT_TRESHOLD 200000 #define DESIRED_UPDATE_RATE_MS 20 // milliseconds // Private types @@ -74,7 +74,7 @@ static xTaskHandle altitudeHoldTaskHandle; static xQueueHandle queue; static AltitudeHoldSettingsData altitudeHoldSettings; static float throttleAlpha = 1.0f; -static float throttle_old = 0.0f; +static float throttle_old = 0.0f; // Private functions static void altitudeHoldTask(void *parameters); @@ -113,19 +113,16 @@ int32_t AltitudeHoldInitialize() MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); float tau; -float altitudeIntegral; -float velocityIntegral; -float decay; -float velocity_decay; -float velocity; float accelAlpha; float velAlpha; bool running = false; + +float velocity; +float velocityIntegral; +float altitudeIntegral; float error; -float switchThrottle; -float smoothed_altitude; -float starting_altitude; float velError; +float derivative; uint32_t timeval; bool posUpdated; @@ -137,10 +134,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltHoldSmoothedData altHold; - AttitudeStateData attitudeState; VelocityStateData velocityData; float dT; - float q[4], Rbe[3][3], fblimit = 0; + float fblimit = 0; float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; @@ -182,6 +178,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) FlightStatusFlightModeGet(&flightMode); altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; if (altitudeHoldFlightMode && !running) { + AttitudeStateData attitudeState; + float q[4], Rbe[3][3]; AttitudeStateGet(&attitudeState); q[0] = attitudeState.q1; q[1] = attitudeState.q2; @@ -197,12 +195,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } else if (initThrottle < 0) { initThrottle = 0; } - error = 0; + error = 0; altitudeHoldDesired.Velocity = 0; altitudeHoldDesired.Altitude = altHold.Altitude; - altitudeIntegral = altHold.Altitude * altitudeHoldSettings.Kp + initThrottle; + altitudeIntegral = initThrottle; velocityIntegral = 0; - running = true; + running = true; } else if (!altitudeHoldFlightMode) { running = false; lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); @@ -241,13 +239,20 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) continue; } + float lastError = error; + error = altitudeHoldDesired.Altitude - altHold.Altitude; + derivative = (error - lastError) / dT; + + velError = altitudeHoldDesired.Velocity - altHold.Velocity; + // Compute altitude and velocity integral - altitudeIntegral += (altitudeHoldDesired.Altitude - altHold.Altitude - fblimit) * altitudeHoldSettings.AltitudeKi * dT; - velocityIntegral += (altitudeHoldDesired.Velocity - altHold.Velocity - fblimit) * altitudeHoldSettings.VelocityKi * dT; + altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudeKi * dT; + velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityKi * dT; + thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently - if ((thisTime - lastUpdateTime)*1000/configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { + if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { continue; } altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; @@ -256,13 +261,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { - stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral - + error * fabsf(error) * altitudeHoldSettings.Kp2 - - altHold.Altitude * altitudeHoldSettings.Kp - - altHold.Velocity * altitudeHoldSettings.Kd - - altHold.Accel * altitudeHoldSettings.Ka; + + error * fabsf(error) * altitudeHoldSettings.Altitude2Kp + + error * altitudeHoldSettings.AltitudeKp + + velError * altitudeHoldSettings.VelocityKp + + derivative * altitudeHoldSettings.AltitudeKd; + // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling + AttitudeStateData attitudeState; + float q[4], Rbe[3][3]; AttitudeStateGet(&attitudeState); q[0] = attitudeState.q1; q[1] = attitudeState.q2; @@ -271,7 +278,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) Quaternion2R(q, Rbe); float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; stabilizationDesired.Throttle /= throttlescale; - stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); + stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); throttle_old = stabilizationDesired.Throttle; fblimit = 0; @@ -309,12 +316,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency) - if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f/DESIRED_UPDATE_RATE_MS) - { - throttleAlpha = (float)DESIRED_UPDATE_RATE_MS/((float)DESIRED_UPDATE_RATE_MS + 1000.0f/(2.0f*M_PI_F*altitudeHoldSettings.ThrottleFilterCutoff)); - } - else - { + if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) { + throttleAlpha = (float)DESIRED_UPDATE_RATE_MS / ((float)DESIRED_UPDATE_RATE_MS + 1000.0f / (2.0f * M_PI_F * altitudeHoldSettings.ThrottleFilterCutoff)); + } else { throttleAlpha = 1.0f; } } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index bc234931c..45e7dd544 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,12 +1,12 @@ Settings for the @ref AltitudeHold module - - + + + - - + From e2a7c6cb25441ce76677dbc9543a5887c01ad784 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 19 Jul 2013 07:57:24 +0000 Subject: [PATCH 019/113] OP-1022 Reset altitude and velocity when stick goes to deadband --- flight/modules/ManualControl/manualcontrol.c | 21 ++++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b888c0476..ef804277c 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -827,10 +827,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) uint8_t throttleExp; static uint8_t flightMode; - static portTickType lastSysTimeAH; static bool zeroed = false; - portTickType thisSysTime; - float dT; FlightStatusFlightModeGet(&flightMode); @@ -848,9 +845,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); - lastSysTimeAH = thisSysTime; + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; @@ -858,8 +855,6 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Altitude = altHold.Altitude; zeroed = false; @@ -867,14 +862,18 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; - altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; + altitudeHoldDesiredData.Altitude = altHold.Altitude; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; - altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; + altitudeHoldDesiredData.Altitude = altHold.Altitude; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 - altitudeHoldDesiredData.Velocity = 0; + if(fabsf(altitudeHoldDesiredData.Velocity)> 1e-3f) + {; + altitudeHoldDesiredData.Velocity = 0; + altitudeHoldDesiredData.Altitude = altHold.Altitude; + } zeroed = true; } From 6b27ff1e27e195af719805095078dd26e71fe482 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 19 Jul 2013 11:28:48 +0000 Subject: [PATCH 020/113] OP-1022 add position lowpass, rework setting uavo, cleanup --- flight/modules/AltitudeHold/altitudehold.c | 36 ++++++++----------- flight/modules/ManualControl/manualcontrol.c | 8 ++--- .../uavobjectdefinition/altholdsmoothed.xml | 3 -- .../altitudeholdsettings.xml | 10 ++---- 4 files changed, 21 insertions(+), 36 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 7e5618165..258daaca0 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -66,7 +66,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define ACCEL_DOWNSAMPLE 4 #define TIMEOUT_TRESHOLD 200000 -#define DESIRED_UPDATE_RATE_MS 20 // milliseconds +#define DESIRED_UPDATE_RATE_MS 100 // milliseconds // Private types // Private variables @@ -113,7 +113,7 @@ int32_t AltitudeHoldInitialize() MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); float tau; -float accelAlpha; +float positionAlpha; float velAlpha; bool running = false; @@ -137,14 +137,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) VelocityStateData velocityData; float dT; float fblimit = 0; - float lastVertVelocity; + portTickType thisTime, lastUpdateTime; UAVObjEvent ev; dT = 0; - lastVertVelocity = 0; timeval = 0; - lastUpdateTime = 0; + lastUpdateTime = 0; // Force update of the settings SettingsUpdatedCb(&ev); // Failsafe handling @@ -215,15 +214,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) VelocityStateGet(&velocityData); altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); - float vertAccel = (velocityData.Down - lastVertVelocity) / dT; - lastVertVelocity = velocityData.Down; - - altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); - altHold.StateUpdateInterval = dT; - - PositionStateDownGet(&altHold.Altitude); - altHold.Altitude = -altHold.Altitude; + float position; + PositionStateDownGet(&position); + altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude; AltHoldSmoothedSet(&altHold); @@ -246,8 +240,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) velError = altitudeHoldDesired.Velocity - altHold.Velocity; // Compute altitude and velocity integral - altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudeKi * dT; - velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityKi * dT; + altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT; + velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT; thisTime = xTaskGetTickCount(); @@ -255,17 +249,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { continue; } - altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; lastUpdateTime = thisTime; // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral - + error * fabsf(error) * altitudeHoldSettings.Altitude2Kp - + error * altitudeHoldSettings.AltitudeKp - + velError * altitudeHoldSettings.VelocityKp - + derivative * altitudeHoldSettings.AltitudeKd; + + error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP] + + velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP] + + derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD]; // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling AttitudeStateData attitudeState; @@ -312,8 +304,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau); - velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); + positionAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.PositionTau); + velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency) if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index ef804277c..0c42f2bcc 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -861,16 +861,16 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude = altHold.Altitude; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude = altHold.Altitude; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 - if(fabsf(altitudeHoldDesiredData.Velocity)> 1e-3f) - {; + if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { + ; altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Altitude = altHold.Altitude; } diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml index b990228b0..1339e1337 100644 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ b/shared/uavobjectdefinition/altholdsmoothed.xml @@ -3,9 +3,6 @@ The output on the kalman filter on altitude. - - - diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 45e7dd544..806d86714 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,14 +1,10 @@ Settings for the @ref AltitudeHold module - - - - - - + + - + From 2da7c24bb7e2cf6e255be934c0f2d21771c598e8 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 19 Nov 2013 21:54:07 +0100 Subject: [PATCH 021/113] OP-1122 OP-1121 close all windows when closing main GCS windows this fixes failures to exit GCS when some dialogs are still open (for example the Waypoint Editor) --- ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index 7a09f1403..1c0b4f7f1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -190,7 +190,8 @@ MainWindow::MainWindow() : MainWindow::~MainWindow() { - if (m_connectionManager) { // Pip + if (m_connectionManager) { + // Pip m_connectionManager->disconnectDevice(); m_connectionManager->suspendPolling(); } @@ -353,6 +354,9 @@ void MainWindow::closeEvent(QCloseEvent *event) saveSettings(m_settings); m_uavGadgetInstanceManager->saveSettings(m_settings); } + + qApp->closeAllWindows(); + event->accept(); } From f942cdf9d6c6151a24b4e1256d4fc78e093f919f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 19 Nov 2013 22:03:03 +0100 Subject: [PATCH 022/113] OP-1122 OP-1120 intial refactoring of code to send waypoints to board - all uavobject sends are now in the same method - removed uncessary updated() calls --- .../src/plugins/opmap/modeluavoproxy.cpp | 308 +++++++++++------- .../src/plugins/opmap/modeluavoproxy.h | 15 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 2 +- .../src/plugins/opmap/opmapgadgetwidget.h | 2 +- 4 files changed, 202 insertions(+), 125 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index bc8c580ed..5f1b1d82c 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -27,7 +27,8 @@ #include "modeluavoproxy.h" #include "extensionsystem/pluginmanager.h" #include -modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) + +ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); @@ -39,86 +40,152 @@ modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObjec pathactionObj = PathAction::GetInstance(objManager); Q_ASSERT(pathactionObj != NULL); } -void modelUavoProxy::modelToObjects() + +void ModelUavoProxy::modelToObjects() { - PathAction *act = NULL; - Waypoint *wp = NULL; - QModelIndex index; - double distance; - double bearing; - double altitude; - int lastaction = -1; + qDebug() << "ModelUAVProxy::modelToObjects"; + // update waypoint and path actions UAV objects + // waypoints are unique and each waypoint has an entry in the UAV waypoint list + // a path action can be referenced by multiple waypoints + // waypoints reference path action by their index in the UAV path action list + // the factoring of path actions (to remove duplicates) happens here... - for (int x = 0; x < myModel->rowCount(); ++x) { - int instances = objManager->getNumInstances(waypointObj->getObjID()); - if (x > instances - 1) { - wp = new Waypoint; - wp->initialize(x, wp->getMetaObject()); - objManager->registerObject(wp); - } else { - wp = Waypoint::GetInstance(objManager, x); + // the UAV waypoint list and path action list are probably not empty + // so we try to reuse existing instances + + // track number of path actions + int actionCount = 0; + + // iterate over waypoints + for (int i = 0; i < myModel->rowCount(); ++i) { + + // Path Actions + + // create action to use as a search criteria + // this object does not need to be deleted as it will either be added to the managed list or deleted later + PathAction *action = new PathAction; + + // get model data + PathAction::DataFields actionData = action->getData(); + modelToPathAction(i, actionData); + + // see if that path action has already been added in this run + PathAction *foundAction = findPathAction(actionData, actionCount); + // TODO this test needs a consistency check as it is unsafe. + // if the find method is buggy and returns false positives then the flight plan sent to the uav is broken! + // the find method should do a "binary" compare instead of a field by field compare + // if a field is added everywhere and not in the compare method then you can start having false positives + if (!foundAction) { + // create or reuse an action instance + action = createPathAction(actionCount, action); + actionCount++; + + // send action update to UAV + action->setData(actionData); + qDebug() << "ModelUAVProxy::modelToObjects - sent action instance :" << action->getInstID(); } - act = new PathAction; - Q_ASSERT(act); - Q_ASSERT(wp); - Waypoint::DataFields waypoint = wp->getData(); - PathAction::DataFields action = act->getData(); - - ///Waypoint object data - index = myModel->index(x, flightDataModel::DISRELATIVE); - distance = myModel->data(index).toDouble(); - index = myModel->index(x, flightDataModel::BEARELATIVE); - bearing = myModel->data(index).toDouble(); - index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); - altitude = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::VELOCITY); - waypoint.Velocity = myModel->data(index).toFloat(); - - waypoint.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); - waypoint.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); - waypoint.Position[Waypoint::POSITION_DOWN] = (-1.0f) * altitude; - - ///PathAction object data - index = myModel->index(x, flightDataModel::MODE); - action.Mode = myModel->data(index).toInt(); - index = myModel->index(x, flightDataModel::MODE_PARAMS0); - action.ModeParameters[0] = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::MODE_PARAMS1); - action.ModeParameters[1] = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::MODE_PARAMS2); - action.ModeParameters[2] = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::MODE_PARAMS3); - action.ModeParameters[3] = myModel->data(index).toFloat(); - - index = myModel->index(x, flightDataModel::CONDITION); - action.EndCondition = myModel->data(index).toInt(); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); - action.ConditionParameters[0] = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); - action.ConditionParameters[1] = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); - action.ConditionParameters[2] = myModel->data(index).toFloat(); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); - action.ConditionParameters[3] = myModel->data(index).toFloat(); - - index = myModel->index(x, flightDataModel::COMMAND); - action.Command = myModel->data(index).toInt(); - index = myModel->index(x, flightDataModel::JUMPDESTINATION); - action.JumpDestination = myModel->data(index).toInt() - 1; - index = myModel->index(x, flightDataModel::ERRORDESTINATION); - action.ErrorDestination = myModel->data(index).toInt() - 1; - - int actionNumber = addAction(act, action, lastaction); - if (actionNumber > lastaction) { - lastaction = actionNumber; + else { + action->deleteLater(); + action = foundAction; + qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID(); } - waypoint.Action = actionNumber; - wp->setData(waypoint); - wp->updated(); + + // Waypoints + + // create or reuse a waypoint instance + Waypoint *waypoint = createWaypoint(i, NULL); + Q_ASSERT(waypoint); + + // get model data + Waypoint::DataFields waypointData = waypoint->getData(); + modelToWaypoint(i, waypointData); + + // connect waypoint to path action + waypointData.Action = action->getInstID(); + + // send waypoint update to UAV + waypoint->setData(waypointData); + qDebug() << "ModelUAVProxy::modelToObjects - sent waypoint instance :" << waypoint->getInstID(); } } -void modelUavoProxy::objectsToModel() +Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { + Waypoint *waypoint = NULL; + int count = objManager->getNumInstances(waypointObj->getObjID()); + if (index < count) { + // reuse object + qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; + waypoint = Waypoint::GetInstance(objManager, index); + if (newWaypoint) { + newWaypoint->deleteLater(); + } + } else if (index < count + 1) { + // create "next" object + qDebug() << "ModelUAVProxy::createWaypoint - created waypoint instance :" << index; + // TODO is there a limit to the number of wp? + waypoint = newWaypoint ? newWaypoint : new Waypoint; + waypoint->initialize(index, waypoint->getMetaObject()); + objManager->registerObject(waypoint); + } + else { + // we can only create the "next" object + // TODO fail in a clean way :( + } + return waypoint; +} + +PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { + PathAction *action = NULL; + int count = objManager->getNumInstances(waypointObj->getObjID()); + if (index < count) { + // reuse object + qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; + action = PathAction::GetInstance(objManager, index); + if (newAction) { + newAction->deleteLater(); + } + } else if (index < count + 1) { + // create "next" object + qDebug() << "ModelUAVProxy::createPathAction - created action instance :" << index; + // TODO is there a limit to the number of path actions? + action = newAction ? newAction : new PathAction; + action->initialize(index, action->getMetaObject()); + objManager->registerObject(action); + } + else { + // we can only create the "next" object + // TODO fail in a clean way :( + } + return action; +} + +PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields& actionData, int actionCount) { + int instancesCount = objManager->getNumInstances(pathactionObj->getObjID()); + int count = actionCount <= instancesCount ? actionCount : instancesCount; + for (int i = 0; i < count; ++i) { + PathAction *action = PathAction::GetInstance(objManager, i); + Q_ASSERT(action); + if (!action) { + continue; + } + PathAction::DataFields fields = action->getData(); + if (fields.Command == actionData.Command + && fields.ConditionParameters[0] == actionData.ConditionParameters[0] + && fields.ConditionParameters[1] == actionData.ConditionParameters[1] + && fields.ConditionParameters[2] == actionData.ConditionParameters[2] + && fields.EndCondition == actionData.EndCondition + && fields.ErrorDestination == actionData.ErrorDestination + && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode + && fields.ModeParameters[0] == actionData.ModeParameters[0] + && fields.ModeParameters[1] == actionData.ModeParameters[1] + && fields.ModeParameters[2] == actionData.ModeParameters[2]) { + return action; + } + } + return NULL; +} + +void ModelUavoProxy::objectsToModel() { Waypoint *wp; Waypoint::DataFields wpfields; @@ -152,7 +219,7 @@ void modelUavoProxy::objectsToModel() index = myModel->index(x, flightDataModel::BEARELATIVE); myModel->setData(index, bearing); index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); - myModel->setData(index, (-1.0f) * wpfields.Position[Waypoint::POSITION_DOWN]); + myModel->setData(index, -wpfields.Position[Waypoint::POSITION_DOWN]); action = PathAction::GetInstance(objManager, wpfields.Action); Q_ASSERT(action); @@ -198,50 +265,51 @@ void modelUavoProxy::objectsToModel() myModel->setData(index, actionfields.ModeParameters[3]); } } -int modelUavoProxy::addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction) -{ - // check if a similar action already exhists - int instances = objManager->getNumInstances(pathactionObj->getObjID()); - for (int x = 0; x < lastaction + 1; ++x) { - PathAction *action = PathAction::GetInstance(objManager, x); - Q_ASSERT(action); - if (!action) { - continue; - } - PathAction::DataFields fields = action->getData(); - if (fields.Command == actionFields.Command - && fields.ConditionParameters[0] == actionFields.ConditionParameters[0] - && fields.ConditionParameters[1] == actionFields.ConditionParameters[1] - && fields.ConditionParameters[2] == actionFields.ConditionParameters[2] - && fields.EndCondition == actionFields.EndCondition - && fields.ErrorDestination == actionFields.ErrorDestination - && fields.JumpDestination == actionFields.JumpDestination - && fields.Mode == actionFields.Mode - && fields.ModeParameters[0] == actionFields.ModeParameters[0] - && fields.ModeParameters[1] == actionFields.ModeParameters[1] - && fields.ModeParameters[2] == actionFields.ModeParameters[2]) { - qDebug() << "ModelUAVProxy:" << "found similar action instance:" << x; - actionObj->deleteLater(); - return x; - } - } - // if we get here it means no similar action was found, we have to create it - if (instances < lastaction + 2) { - actionObj->initialize(instances, actionObj->getMetaObject()); - objManager->registerObject(actionObj); - actionObj->setData(actionFields); - actionObj->updated(); - qDebug() << "ModelUAVProxy:" << "created new action instance:" << instances; - return lastaction + 1; - } else { - PathAction *action = PathAction::GetInstance(objManager, lastaction + 1); - Q_ASSERT(action); - action->setData(actionFields); - action->updated(); - actionObj->deleteLater(); - qDebug() << "ModelUAVProxy:" << "reused action instance:" << lastaction + 1; - return lastaction + 1; - } - return -1; // error we should never get here +void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data) { + QModelIndex index = myModel->index(i, flightDataModel::MODE); + data.Mode = myModel->data(index).toInt(); + index = myModel->index(i, flightDataModel::MODE_PARAMS0); + data.ModeParameters[0] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::MODE_PARAMS1); + data.ModeParameters[1] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::MODE_PARAMS2); + data.ModeParameters[2] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::MODE_PARAMS3); + data.ModeParameters[3] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::CONDITION); + data.EndCondition = myModel->data(index).toInt(); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); + data.ConditionParameters[0] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS1); + data.ConditionParameters[1] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); + data.ConditionParameters[2] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS3); + data.ConditionParameters[3] = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::COMMAND); + data.Command = myModel->data(index).toInt(); + index = myModel->index(i, flightDataModel::JUMPDESTINATION); + data.JumpDestination = myModel->data(index).toInt() - 1; + index = myModel->index(i, flightDataModel::ERRORDESTINATION); + data.ErrorDestination = myModel->data(index).toInt() - 1; } + +void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { + double distance, bearing, altitude, velocity; + + QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE); + distance = myModel->data(index).toDouble(); + index = myModel->index(i, flightDataModel::BEARELATIVE); + bearing = myModel->data(index).toDouble(); + index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); + altitude = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::VELOCITY); + velocity = myModel->data(index).toFloat(); + + data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); + data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); + data.Position[Waypoint::POSITION_DOWN] = -altitude; + data.Velocity = velocity; +} + diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 9b23cbcee..8c7b94e88 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -32,19 +32,28 @@ #include "pathaction.h" #include "waypoint.h" -class modelUavoProxy : public QObject { +class ModelUavoProxy : public QObject { Q_OBJECT + public: - explicit modelUavoProxy(QObject *parent, flightDataModel *model); - int addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction); + explicit ModelUavoProxy(QObject *parent, flightDataModel *model); + public slots: void modelToObjects(); void objectsToModel(); + private: UAVObjectManager *objManager; Waypoint *waypointObj; PathAction *pathactionObj; flightDataModel *myModel; + + Waypoint *createWaypoint(int index, Waypoint *newWaypoint); + void modelToWaypoint(int i, Waypoint::DataFields &waypointData); + + PathAction *findPathAction(const PathAction::DataFields& actionFields, int actionCount); + PathAction *createPathAction(int index, PathAction *newAction); + void modelToPathAction(int i, PathAction::DataFields &actionData); }; #endif // MODELUAVOPROXY_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 4fa7c1bba..8f5f22d9d 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -225,7 +225,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) mapProxy = new modelMapProxy(this, m_map, model, selectionModel); table->setModel(model, selectionModel); waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel); - UAVProxy = new modelUavoProxy(this, model); + UAVProxy = new ModelUavoProxy(this, model); connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects())); connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel())); #endif diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index 5c5e178d2..c50c221d8 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -233,7 +233,7 @@ private: QPointer waypoint_edit_dialog; QStandardItemModel wayPoint_treeView_model; mapcontrol::WayPointItem *m_mouse_waypoint; - QPointer UAVProxy; + QPointer UAVProxy; QMutex m_map_mutex; bool m_telemetry_connected; QAction *closeAct1; From 3efea325076ed4918d09d82babbc30e434a8817f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 19 Nov 2013 23:30:56 +0100 Subject: [PATCH 023/113] OP-1122 OP-1120 fixed regressions introduced by previous refactoring of waypoint to board transmission - re-introduced calls to updated() which are needed for some reason I don't yet understand - without the updated() call the waypoint are not sent to the board (or at least they get overwritten by the board waypoint) - with the updated() call i see lots of "!!!!!! Making request for an object: Waypoint" for which a request is already in progress!!!!!! messages. --- ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 5f1b1d82c..cab71749e 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -82,6 +82,7 @@ void ModelUavoProxy::modelToObjects() // send action update to UAV action->setData(actionData); + action->updated(); qDebug() << "ModelUAVProxy::modelToObjects - sent action instance :" << action->getInstID(); } else { @@ -105,6 +106,7 @@ void ModelUavoProxy::modelToObjects() // send waypoint update to UAV waypoint->setData(waypointData); + waypoint->updated(); qDebug() << "ModelUAVProxy::modelToObjects - sent waypoint instance :" << waypoint->getInstID(); } } @@ -136,7 +138,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *action = NULL; - int count = objManager->getNumInstances(waypointObj->getObjID()); + int count = objManager->getNumInstances(actionObj->getObjID()); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; From aff543561ba43a3d271a87596ff1ee180cc3788c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 20 Nov 2013 22:25:19 +0100 Subject: [PATCH 024/113] OP-1122 OP-1120 refactored code to retreive board waypoints + fixed compilation error --- .../src/plugins/opmap/modeluavoproxy.cpp | 171 +++++++++--------- .../src/plugins/opmap/modeluavoproxy.h | 6 +- 2 files changed, 92 insertions(+), 85 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index cab71749e..9dbc1fbfb 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -138,7 +138,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *action = NULL; - int count = objManager->getNumInstances(actionObj->getObjID()); + int count = objManager->getNumInstances(pathactionObj->getObjID()); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; @@ -161,7 +161,7 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { return action; } -PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields& actionData, int actionCount) { +PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { int instancesCount = objManager->getNumInstances(pathactionObj->getObjID()); int count = actionCount <= instancesCount ? actionCount : instancesCount; for (int i = 0; i < count; ++i) { @@ -189,85 +189,71 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields& actionD void ModelUavoProxy::objectsToModel() { - Waypoint *wp; - Waypoint::DataFields wpfields; - PathAction *action; - QModelIndex index; - double distance; - double bearing; - - PathAction::DataFields actionfields; - myModel->removeRows(0, myModel->rowCount()); - for (int x = 0; x < objManager->getNumInstances(waypointObj->getObjID()); ++x) { - wp = Waypoint::GetInstance(objManager, x); - Q_ASSERT(wp); - if (!wp) { + + int instanceCount = objManager->getNumInstances(waypointObj->getObjID()); + for (int i = 0; i < instanceCount; ++i) { + Waypoint *waypoint = Waypoint::GetInstance(objManager, i); + Q_ASSERT(waypoint); + if (!waypoint) { continue; } - wpfields = wp->getData(); - myModel->insertRow(x); - index = myModel->index(x, flightDataModel::VELOCITY); - myModel->setData(index, wpfields.Velocity); - distance = sqrt(wpfields.Position[Waypoint::POSITION_NORTH] * wpfields.Position[Waypoint::POSITION_NORTH] + - wpfields.Position[Waypoint::POSITION_EAST] * wpfields.Position[Waypoint::POSITION_EAST]); - bearing = atan2(wpfields.Position[Waypoint::POSITION_EAST], wpfields.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; + myModel->insertRow(i); - if (bearing != bearing) { - bearing = 0; - } - index = myModel->index(x, flightDataModel::DISRELATIVE); - myModel->setData(index, distance); - index = myModel->index(x, flightDataModel::BEARELATIVE); - myModel->setData(index, bearing); - index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); - myModel->setData(index, -wpfields.Position[Waypoint::POSITION_DOWN]); + Waypoint::DataFields waypointData = waypoint->getData(); + waypointToModel(i, waypointData); - action = PathAction::GetInstance(objManager, wpfields.Action); + PathAction *action = PathAction::GetInstance(objManager, waypointData.Action); Q_ASSERT(action); if (!action) { continue; } - actionfields = action->getData(); - index = myModel->index(x, flightDataModel::ISRELATIVE); - myModel->setData(index, true); - - index = myModel->index(x, flightDataModel::COMMAND); - myModel->setData(index, actionfields.Command); - - index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); - myModel->setData(index, actionfields.ConditionParameters[0]); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); - myModel->setData(index, actionfields.ConditionParameters[1]); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); - myModel->setData(index, actionfields.ConditionParameters[2]); - index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); - myModel->setData(index, actionfields.ConditionParameters[3]); - - index = myModel->index(x, flightDataModel::CONDITION); - myModel->setData(index, actionfields.EndCondition); - - index = myModel->index(x, flightDataModel::ERRORDESTINATION); - myModel->setData(index, actionfields.ErrorDestination + 1); - - index = myModel->index(x, flightDataModel::JUMPDESTINATION); - myModel->setData(index, actionfields.JumpDestination + 1); - - index = myModel->index(x, flightDataModel::MODE); - myModel->setData(index, actionfields.Mode); - - index = myModel->index(x, flightDataModel::MODE_PARAMS0); - myModel->setData(index, actionfields.ModeParameters[0]); - index = myModel->index(x, flightDataModel::MODE_PARAMS1); - myModel->setData(index, actionfields.ModeParameters[1]); - index = myModel->index(x, flightDataModel::MODE_PARAMS2); - myModel->setData(index, actionfields.ModeParameters[2]); - index = myModel->index(x, flightDataModel::MODE_PARAMS3); - myModel->setData(index, actionfields.ModeParameters[3]); + PathAction::DataFields actionData = action->getData(); + pathActionToModel(i, actionData); } } +void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { + double distance, bearing, altitude, velocity; + + QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE); + distance = myModel->data(index).toDouble(); + index = myModel->index(i, flightDataModel::BEARELATIVE); + bearing = myModel->data(index).toDouble(); + index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); + altitude = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::VELOCITY); + velocity = myModel->data(index).toFloat(); + + data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / (180 * M_PI)); + data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / (180 * M_PI)); + data.Position[Waypoint::POSITION_DOWN] = -altitude; + data.Velocity = velocity; +} + +void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) { + + double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] + + data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]); + + double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * (180 / M_PI); + if (bearing != bearing) { + bearing = 0; + } + + double altitude = -data.Position[Waypoint::POSITION_DOWN]; + + QModelIndex index = myModel->index(i, flightDataModel::VELOCITY); + myModel->setData(index, data.Velocity); + index = myModel->index(i, flightDataModel::DISRELATIVE); + myModel->setData(index, distance); + index = myModel->index(i, flightDataModel::BEARELATIVE); + myModel->setData(index, bearing); + index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); + myModel->setData(index, altitude); +} + void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data) { QModelIndex index = myModel->index(i, flightDataModel::MODE); data.Mode = myModel->data(index).toInt(); @@ -297,21 +283,40 @@ void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data) { data.ErrorDestination = myModel->data(index).toInt() - 1; } -void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { - double distance, bearing, altitude, velocity; +void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { + QModelIndex index = myModel->index(i, flightDataModel::ISRELATIVE); + myModel->setData(index, true); - QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE); - distance = myModel->data(index).toDouble(); - index = myModel->index(i, flightDataModel::BEARELATIVE); - bearing = myModel->data(index).toDouble(); - index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); - altitude = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::VELOCITY); - velocity = myModel->data(index).toFloat(); + index = myModel->index(i, flightDataModel::COMMAND); + myModel->setData(index, data.Command); - data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); - data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); - data.Position[Waypoint::POSITION_DOWN] = -altitude; - data.Velocity = velocity; + index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); + myModel->setData(index, data.ConditionParameters[0]); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS1); + myModel->setData(index, data.ConditionParameters[1]); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); + myModel->setData(index, data.ConditionParameters[2]); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS3); + myModel->setData(index, data.ConditionParameters[3]); + + index = myModel->index(i, flightDataModel::CONDITION); + myModel->setData(index, data.EndCondition); + + index = myModel->index(i, flightDataModel::ERRORDESTINATION); + myModel->setData(index, data.ErrorDestination + 1); + + index = myModel->index(i, flightDataModel::JUMPDESTINATION); + myModel->setData(index, data.JumpDestination + 1); + + index = myModel->index(i, flightDataModel::MODE); + myModel->setData(index, data.Mode); + + index = myModel->index(i, flightDataModel::MODE_PARAMS0); + myModel->setData(index, data.ModeParameters[0]); + index = myModel->index(i, flightDataModel::MODE_PARAMS1); + myModel->setData(index, data.ModeParameters[1]); + index = myModel->index(i, flightDataModel::MODE_PARAMS2); + myModel->setData(index, data.ModeParameters[2]); + index = myModel->index(i, flightDataModel::MODE_PARAMS3); + myModel->setData(index, data.ModeParameters[3]); } - diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 8c7b94e88..38a88d1e8 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -49,11 +49,13 @@ private: flightDataModel *myModel; Waypoint *createWaypoint(int index, Waypoint *newWaypoint); - void modelToWaypoint(int i, Waypoint::DataFields &waypointData); + void modelToWaypoint(int i, Waypoint::DataFields &data); + void waypointToModel(int i, Waypoint::DataFields &data); PathAction *findPathAction(const PathAction::DataFields& actionFields, int actionCount); PathAction *createPathAction(int index, PathAction *newAction); - void modelToPathAction(int i, PathAction::DataFields &actionData); + void modelToPathAction(int i, PathAction::DataFields &data); + void pathActionToModel(int i, PathAction::DataFields &data); }; #endif // MODELUAVOPROXY_H From 87725ded48414c11c4cf3a45f745efde39b2b30f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 21 Nov 2013 00:22:19 +0100 Subject: [PATCH 025/113] OP-1122 OP-1123 fixed assertion errors when loading waypoint file in wp editor --- ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp | 2 +- ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index d16c9b381..9fb0787d5 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -469,7 +469,7 @@ bool flightDataModel::insertRows(int row, int count, const QModelIndex & /*paren bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*parent*/) { - if (row < 0) { + if (row < 0 || count <= 0) { return false; } beginRemoveRows(QModelIndex(), row, row + count - 1); diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp index 2dac1b0ac..ab829da75 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp @@ -345,7 +345,10 @@ void modelMapProxy::createWayPoint(internals::PointLatLng coord) index = model->index(model->rowCount() - 1, flightDataModel::ERRORDESTINATION, QModelIndex()); model->setData(index, 1, Qt::EditRole); } + void modelMapProxy::deleteAll() { - model->removeRows(0, model->rowCount(), QModelIndex()); + if (model->rowCount() > 0) { + model->removeRows(0, model->rowCount(), QModelIndex()); + } } From 1469277f96cda9816af96417e9fde0663449b0f1 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 23 Nov 2013 14:59:24 +0100 Subject: [PATCH 026/113] OP-1122 OP-1125 removed dead code from fw uavtalk (first fw commit ;) --- flight/uavtalk/inc/uavtalk.h | 3 --- flight/uavtalk/uavtalk.c | 49 +----------------------------------- 2 files changed, 1 insertion(+), 51 deletions(-) diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index 3067b502c..b3a638bfc 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -54,9 +54,6 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection); int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); -int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId); -int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId); -int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len); UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte); UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle); diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index c20bc9690..aeede370e 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -675,54 +675,6 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) return 0; } -/** - * Send a ACK through the telemetry link. - * \param[in] connectionHandle UAVTalkConnection to be used - * \param[in] objId Object ID to send a NACK for - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId) -{ - UAVTalkConnectionData *connection; - - CHECKCONHANDLE(connectionHandle, connection, return -1); - - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); - - return ret; -} - -/** - * Send a NACK through the telemetry link. - * \param[in] connectionHandle UAVTalkConnection to be used - * \param[in] objId Object ID to send a NACK for - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) -{ - UAVTalkConnectionData *connection; - - CHECKCONHANDLE(connectionHandle, connection, return -1); - - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - int32_t ret = sendNack(connection, objId); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); - - return ret; -} - /** * Get the object ID of the current packet. * \param[in] connectionHandle UAVTalkConnection to be used @@ -798,6 +750,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, case UAVTALK_TYPE_OBJ_REQ: // Send requested object if message is of type OBJ_REQ if (obj == 0) { + // Transmit NACK sendNack(connection, objId); } else { sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); From 2f0974fda942f28b09b866991121aef9cb939cce Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 26 Nov 2013 01:27:25 +0100 Subject: [PATCH 027/113] OP-1122 OP-1125 uavtalk - fixed a number of issues concerning acked multi instance messages : - all messages now systematically include an instance ID (fixes parsing of messages containing unknown object) - added missing NACK of object request on fight side - added more logging and warnings - misc fixes and cleanups this commit affects both the firmware and GCS (need to flash new version of firmware) --- flight/uavtalk/uavtalk.c | 199 +++++------- .../src/plugins/uavobjects/uavobject.cpp | 5 +- .../src/plugins/uavtalk/telemetry.cpp | 101 ++++-- .../src/plugins/uavtalk/telemetry.h | 7 +- .../src/plugins/uavtalk/uavtalk.cpp | 297 ++++++++++-------- .../src/plugins/uavtalk/uavtalk.h | 15 +- .../src/plugins/uavtalk/uavtalk.pro | 19 +- 7 files changed, 356 insertions(+), 287 deletions(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index aeede370e..1dcfd8242 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -34,10 +34,9 @@ // Private functions -static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); -static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); -static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); -static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId); +static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout); +static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); +static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length); static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId); @@ -213,7 +212,8 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandl UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle, connection, return -1); - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout); + + return objectTransaction(connection, UAVTALK_TYPE_OBJ_REQ, obj, instId, timeout); } /** @@ -231,11 +231,12 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle, connection, return -1); + // Send object if (acked == 1) { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs); + return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK, obj, instId, timeoutMs); } else { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs); + return objectTransaction(connection, UAVTALK_TYPE_OBJ, obj, instId, timeoutMs); } } @@ -254,27 +255,29 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle, connection, return -1); + // Send object if (acked == 1) { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs); + return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK_TS, obj, instId, timeoutMs); } else { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs); + return objectTransaction(connection, UAVTALK_TYPE_OBJ_TS, obj, instId, timeoutMs); } } /** * Execute the requested transaction on an object. * \param[in] connection UAVTalkConnection to be used - * \param[in] obj Object - * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type * UAVTALK_TYPE_OBJ: send object, * UAVTALK_TYPE_OBJ_REQ: request object update * UAVTALK_TYPE_OBJ_ACK: send object with an ack + * \param[in] obj Object + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. + * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately * \return 0 Success * \return -1 Failure */ -static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) +static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs) { int32_t respReceived; @@ -286,7 +289,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); connection->respObj = obj; connection->respInstId = instId; - sendObject(connection, obj, instId, type); + sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); // Wait for response (or timeout) respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS); @@ -305,7 +308,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle } } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) { xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - sendObject(connection, obj, instId, type); + sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); return 0; } else { @@ -326,6 +329,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle CHECKCONHANDLE(connectionHandle, connection, return -1); UAVTalkInputProcessor *iproc = &connection->iproc; + ++connection->stats.rxBytes; if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) { @@ -338,6 +342,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle // Receive state machine switch (iproc->state) { case UAVTALK_STATE_SYNC: + if (rxbyte != UAVTALK_SYNC_VAL) { break; } @@ -408,19 +413,18 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle // Determine data length if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { iproc->length = 0; - iproc->instanceLength = 0; } else { if (iproc->obj) { iproc->length = UAVObjGetNumBytes(iproc->obj); - iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); } else { - // We don't know if it's a multi-instance object, so just assume it's 0. - iproc->instanceLength = 0; iproc->length = iproc->packet_size - iproc->rxPacketLength; } iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; } + // Message always contain an instance ID + iproc->instanceLength = 2; + // Check length and determine next state if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { connection->stats.rxErrors++; @@ -435,28 +439,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } - iproc->instId = 0; - if (iproc->type == UAVTALK_TYPE_NACK) { - // If this is a NACK, we skip to Checksum - iproc->state = UAVTALK_STATE_CS; - } - // Check if this is a single instance object (i.e. if the instance ID field is coming next) - else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) { - iproc->state = UAVTALK_STATE_INSTID; - } - // Check if this is a single instance and has a timestamp in it - else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) { - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } else { - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) { - iproc->state = UAVTALK_STATE_DATA; - } else { - iproc->state = UAVTALK_STATE_CS; - } - } iproc->rxCount = 0; + iproc->instId = 0; + iproc->state = UAVTALK_STATE_INSTID; break; @@ -713,8 +698,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // Lock xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - // Get the handle to the Object. Will be zero - // if object does not exist. + // Get the handle to the Object. + // Will be zero if object does not exist. + // Warning : + // Here we ask for instance ID 0 without taking into account the provided instId + // The provided instId will be used later when packing, unpacking, etc... + // TODO the above should be fixed as it is cumbersome and error prone obj = UAVObjGetByID(objId); // Process message type @@ -724,9 +713,15 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // All instances, not allowed for OBJ messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(obj, instId, data); - // Check if an ack is pending - updateAck(connection, obj, instId); + if (UAVObjUnpack(obj, instId, data)) { + // Check if an ack is pending OBJ_ACK below + // TODO is it necessary to do that check? + // TODO if yes, why is the same check not done for OBJ_ACK below? + updateAck(connection, obj, instId); + } + else { + ret = -1; + } } else { ret = -1; } @@ -738,22 +733,30 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! if (UAVObjUnpack(obj, instId, data) == 0) { - // Transmit ACK - sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + // Object updated or created, transmit ACK + sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL); } else { ret = -1; } } else { ret = -1; } + if (ret == -1) { + // failed to update object, transmit NACK + sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); + } break; case UAVTALK_TYPE_OBJ_REQ: - // Send requested object if message is of type OBJ_REQ - if (obj == 0) { - // Transmit NACK - sendNack(connection, objId); + // Check if requested object exists + if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) { + // Object found, transmit it + sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj); } else { - sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); + ret = -1; + } + if (ret == -1) { + // failed to send object, transmit NACK + sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); } break; case UAVTALK_TYPE_NACK: @@ -762,7 +765,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, case UAVTALK_TYPE_ACK: // All instances, not allowed for ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { - // Check if an ack is pending + // Check if an ACK is pending updateAck(connection, obj, instId); } else { ret = -1; @@ -796,19 +799,22 @@ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint1 /** * Send an object through the telemetry link. * \param[in] connection UAVTalkConnection to be used - * \param[in] obj Object handle to send - * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances * \param[in] type Transaction type + * \param[in] objId The object ID + * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances + * \param[in] obj Object handle to send (null when type is NACK) * \return 0 Success * \return -1 Failure */ -static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) +static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj) { uint32_t numInst; uint32_t n; + // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences. + // If all instances are requested and this is a single instance object, force instance ID to zero - if (instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj)) { + if ((obj != NULL) && (instId == UAVOBJ_ALL_INSTANCES) && UAVObjIsSingleInstance(obj)) { instId = 0; } @@ -819,17 +825,17 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u numInst = UAVObjGetNumInstances(obj); // Send all instances for (n = 0; n < numInst; ++n) { - sendSingleObject(connection, obj, n, type); + sendSingleObject(connection, type, objId, n, obj); } return 0; } else { - return sendSingleObject(connection, obj, instId, type); + return sendSingleObject(connection, type, objId, instId, obj); } } else if (type == UAVTALK_TYPE_OBJ_REQ) { - return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ); - } else if (type == UAVTALK_TYPE_ACK) { + return sendSingleObject(connection, UAVTALK_TYPE_OBJ_REQ, objId, instId, obj); + } else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) { if (instId != UAVOBJ_ALL_INSTANCES) { - return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK); + return sendSingleObject(connection, type, objId, instId, obj); } else { return -1; } @@ -841,24 +847,26 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u /** * Send an object through the telemetry link. * \param[in] connection UAVTalkConnection to be used - * \param[in] obj Object handle to send - * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead) * \param[in] type Transaction type + * \param[in] objId The object ID + * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use +() instead) + * \param[in] obj Object handle to send (null when type is NACK) * \return 0 Success * \return -1 Failure */ -static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) +static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj) { int32_t length; int32_t dataOffset; - uint32_t objId; + + // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences. if (!connection->outStream) { return -1; } // Setup type and object id fields - objId = UAVObjGetID(obj); connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte connection->txBuffer[1] = type; // data length inserted here below @@ -866,15 +874,12 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); + dataOffset = 8; - // Setup instance ID if one is required - if (UAVObjIsSingleInstance(obj)) { - dataOffset = 8; - } else { - connection->txBuffer[8] = (uint8_t)(instId & 0xFF); - connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset = 10; - } + // The instance ID is always sent + connection->txBuffer[8] = (uint8_t)(instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); + dataOffset += 2; // Add timestamp when the transaction type is appropriate if (type & UAVTALK_TIMESTAMPED) { @@ -885,7 +890,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle } // Determine data length - if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) { + if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) { length = 0; } else { length = UAVObjGetNumBytes(obj); @@ -924,50 +929,6 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle return 0; } -/** - * Send a NACK through the telemetry link. - * \param[in] connection UAVTalkConnection to be used - * \param[in] objId Object ID to send a NACK for - * \return 0 Success - * \return -1 Failure - */ -static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId) -{ - int32_t dataOffset; - - if (!connection->outStream) { - return -1; - } - - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = UAVTALK_TYPE_NACK; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - - dataOffset = 8; - - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); - - // Calculate checksum - connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset); - - uint16_t tx_msg_len = dataOffset + UAVTALK_CHECKSUM_LENGTH; - int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); - - if (rc == tx_msg_len) { - // Update stats - connection->stats.txBytes += tx_msg_len; - } - - // Done - return 0; -} - /** * @} * @} diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index d795e8cb3..cca8afb23 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -436,6 +436,7 @@ QString UAVObject::toString() QString sout; sout.append(toStringBrief()); + sout.append('\n'); sout.append(toStringData()); return sout; } @@ -447,12 +448,12 @@ QString UAVObject::toStringBrief() { QString sout; - sout.append(QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n") + sout.append(QString("%1 (ID: %2:%3, %4bytes, %5)") .arg(getName()) .arg(getObjID()) .arg(getInstID()) .arg(getNumBytes()) - .arg(isSingleInstance())); + .arg(isSingleInstance() ? "single" : "multiple")); return sout; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 15dc5ad47..f8674acff 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -65,9 +65,8 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) Telemetry::~Telemetry() { - for (QMap::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) { - delete itr.value(); - } + closeAllTransactions(); + } /** @@ -122,6 +121,7 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs) */ void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask) { + // TODO why connect systematically to all instances? QList objs = objMngr->getObjectInstances(obj->getObjID()); for (int n = 0; n < objs.length(); ++n) { // Disconnect all @@ -211,21 +211,25 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType) void Telemetry::transactionCompleted(UAVObject *obj, bool success) { // Lookup the transaction in the transaction map. - quint32 objId = obj->getObjID(); - - QMap::iterator itr = transMap.find(objId); - if (itr != transMap.end()) { - ObjectTransactionInfo *transInfo = itr.value(); + ObjectTransactionInfo *transInfo = findTransaction(obj); + if (transInfo) { // Remove this transaction as it's complete. transInfo->timer->stop(); - transMap.remove(objId); + closeTransaction(transInfo); delete transInfo; // Send signal + if (success) { +#ifdef VERBOSE_TELEMETRY + qDebug() << "Telemetry - transaction successful for object" << obj->toStringBrief(); +#endif + } else { + qWarning() << "Telemetry - !!! transaction failed for object" << obj->toStringBrief(); + } obj->emitTransactionCompleted(success); // Process new object updates from queue processObjectQueue(); } else { - qDebug() << "Error: received a transaction completed when did not expect it."; + qWarning() << "Telemetry - Error: received a transaction completed when did not expect it for" << obj->toStringBrief(); } } @@ -237,6 +241,9 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) transInfo->timer->stop(); // Check if more retries are pending if (transInfo->retriesRemaining > 0) { +#ifdef VERBOSE_TELEMETRY + qDebug() << QString("Telemetry - transaction timed out for object %1, retrying...").arg(transInfo->obj->toStringBrief()); +#endif --transInfo->retriesRemaining; processObjectTransaction(transInfo); ++txRetries; @@ -246,9 +253,11 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) // Terminate transaction utalk->cancelTransaction(transInfo->obj); // Send signal + qWarning() << QString("Telemetry - !!! transaction timed out for object %1").arg(transInfo->obj->toStringBrief()); transInfo->obj->emitTransactionCompleted(false); // Remove this transaction as it's complete. - transMap.remove(transInfo->obj->getObjID()); + // FIXME : also remove transaction from UAVTalk transaction map + closeTransaction(transInfo); delete transInfo; // Process new object updates from queue processObjectQueue(); @@ -263,8 +272,14 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) { // Initiate transaction if (transInfo->objRequest) { +#ifdef VERBOSE_TELEMETRY + qDebug() << QString("Telemetry - sending object request for %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : ""); +#endif utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); } else { +#ifdef VERBOSE_TELEMETRY + qDebug() << QString("Telemetry - sending object %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : ""); +#endif utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); } // Start timer if a response is expected @@ -272,7 +287,7 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) transInfo->timer->start(REQ_TIMEOUT_MS); } else { // Otherwise, remove this transaction as it's complete. - transMap.remove(transInfo->obj->getObjID()); + closeTransaction(transInfo); delete transInfo; } } @@ -293,14 +308,15 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn objPriorityQueue.enqueue(objInfo); } else { ++txErrors; + qWarning() << QString("Telemetry - !!! priority event queue is full, event lost (%1)").arg(obj->toStringBrief()); obj->emitTransactionCompleted(false); - qDebug() << tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName()); } } else { if (objQueue.length() < MAX_QUEUE_SIZE) { objQueue.enqueue(objInfo); } else { ++txErrors; + qWarning() << QString("Telemetry - !!! event queue is full, event lost (%1)").arg(obj->toStringBrief()); obj->emitTransactionCompleted(false); } } @@ -340,9 +356,10 @@ void Telemetry::processObjectQueue() UAVObject::Metadata metadata = objInfo.obj->getMetadata(); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) { - QMap::iterator itr = transMap.find(objInfo.obj->getObjID()); - if (itr != transMap.end()) { - qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!"; + if (findTransaction(objInfo.obj)) { + qWarning() << QString("Telemetry - !!! Making request for an object: %1 for which a request is already in progress").arg(objInfo.obj->toStringBrief()); + objInfo.obj->emitTransactionCompleted(false); + return; } UAVObject::Metadata metadata = objInfo.obj->getMetadata(); ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this); @@ -357,7 +374,7 @@ void Telemetry::processObjectQueue() } transInfo->telem = this; // Insert the transaction into the transaction map. - transMap.insert(objInfo.obj->getObjID(), transInfo); + openTransaction(transInfo); processObjectTransaction(transInfo); } @@ -396,6 +413,7 @@ void Telemetry::processPeriodicUpdates() qint32 elapsedMs = 0; QTime time; qint32 offset; + bool allInstances; for (int n = 0; n < objList.length(); ++n) { objinfo = &objList[n]; // If object is configured for periodic updates @@ -408,7 +426,8 @@ void Telemetry::processPeriodicUpdates() objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset; // Send object time.start(); - processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, true, false); + allInstances = !objinfo->obj->isSingleInstance(); + processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false); elapsedMs = time.elapsed(); // Update timeToNextUpdateMs with the elapsed delay of sending the object; timeToNextUpdateMs += elapsedMs; @@ -496,7 +515,6 @@ void Telemetry::objectUnpacked(UAVObject *obj) void Telemetry::updateRequested(UAVObject *obj) { QMutexLocker locker(mutex); - processObjectUpdates(obj, EV_UPDATE_REQ, false, true); } @@ -514,6 +532,51 @@ void Telemetry::newInstance(UAVObject *obj) registerObject(obj); } +ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) +{ + // Lookup the transaction in the transaction map + QMap *objTransactions = transMap.value(obj->getObjID()); + if (objTransactions != NULL) { + return objTransactions->value(obj->getInstID()); + } + return NULL; +} + +void Telemetry::openTransaction(ObjectTransactionInfo *trans) +{ + QMap *objTransactions = transMap.value(trans->obj->getObjID()); + if (objTransactions == NULL) { + objTransactions = new QMap(); + transMap.insert(trans->obj->getObjID(), objTransactions); + } + objTransactions->insert(trans->obj->getInstID(), trans); +} + +void Telemetry::closeTransaction(ObjectTransactionInfo *trans) +{ + QMap *objTransactions = transMap.value(trans->obj->getObjID()); + if (objTransactions != NULL) { + objTransactions->remove(trans->obj->getInstID()); + // Keep the map even if it is empty + // There are at most 100 different object IDs... + } +} + +void Telemetry::closeAllTransactions() +{ + foreach(quint32 objId, transMap.keys()) { + QMap *objTransactions = transMap.value(objId); + foreach(quint32 instId, objTransactions->keys()) { + ObjectTransactionInfo *trans = objTransactions->value(instId); + qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief(); + objTransactions->remove(instId); + delete trans; + } + transMap.remove(objId); + delete objTransactions; + } +} + ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent) { obj = 0; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 751aeaeae..4462ec828 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h @@ -118,7 +118,7 @@ private: QList objList; QQueue objQueue; QQueue objPriorityQueue; - QMaptransMap; + QMap *> transMap; QMutex *mutex; QTimer *updateTimer; QTimer *statsTimer; @@ -136,6 +136,11 @@ private: void processObjectTransaction(ObjectTransactionInfo *transInfo); void processObjectQueue(); + ObjectTransactionInfo *findTransaction(UAVObject *obj); + void openTransaction(ObjectTransactionInfo *trans); + void closeTransaction(ObjectTransactionInfo *trans); + void closeAllTransactions(); + private slots: void objectUpdatedAuto(UAVObject *obj); void objectUpdatedManual(UAVObject *obj); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 366aa56e4..9df149d34 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -94,6 +94,7 @@ UAVTalk::~UAVTalk() // According to Qt, it is not necessary to disconnect upon // object deletion. // disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); + closeAllTransactions(); } @@ -154,7 +155,7 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) { QMutexLocker locker(mutex); - return objectTransaction(obj, TYPE_OBJ_REQ, allInstances); + return objectTransaction(TYPE_OBJ_REQ, obj, allInstances); } /** @@ -169,9 +170,9 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) QMutexLocker locker(mutex); if (acked) { - return objectTransaction(obj, TYPE_OBJ_ACK, allInstances); + return objectTransaction(TYPE_OBJ_ACK, obj, allInstances); } else { - return objectTransaction(obj, TYPE_OBJ, allInstances); + return objectTransaction(TYPE_OBJ, obj, allInstances); } } @@ -181,15 +182,14 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) void UAVTalk::cancelTransaction(UAVObject *obj) { QMutexLocker locker(mutex); - quint32 objId = obj->getObjID(); if (io.isNull()) { return; } - QMap::iterator itr = transMap.find(objId); - if (itr != transMap.end()) { - transMap.remove(objId); - delete itr.value(); + Transaction *trans = findTransaction(obj); + if (trans != NULL) { + closeTransaction(trans); + delete trans; } } @@ -203,21 +203,24 @@ void UAVTalk::cancelTransaction(UAVObject *obj) * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances) +bool UAVTalk::objectTransaction(quint8 type, UAVObject *obj, bool allInstances) { + Q_ASSERT(obj); // Send object depending on if a response is needed + // transactions of TYPE_OBJ_REQ are acked by the response + quint16 instId = allInstances ? ALL_INSTANCES : obj->getInstID(); if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { - if (transmitObject(obj, type, allInstances)) { + if (transmitObject(type, obj->getObjID(), instId, obj)) { Transaction *trans = new Transaction(); trans->obj = obj; trans->allInstances = allInstances; - transMap.insert(obj->getObjID(), trans); + openTransaction(trans); return true; } else { return false; } } else if (type == TYPE_OBJ) { - return transmitObject(obj, TYPE_OBJ, allInstances); + return transmitObject(type, obj->getObjID(), instId, obj); } else { return false; } @@ -233,7 +236,8 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // Update stats stats.rxBytes++; - rxPacketLength++; // update packet byte count + // update packet byte count + rxPacketLength++; if (useUDPMirror) { rxDataArray.append(rxbyte); @@ -332,12 +336,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // Determine data length if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) { rxLength = 0; - rxInstanceLength = 0; } else { rxLength = rxObj->getNumBytes(); - rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2); } + // The instance ID is always sent + rxInstanceLength = 2; + // Check length and determine next state if (rxLength >= MAX_PAYLOAD_LENGTH) { stats.rxErrors++; @@ -347,39 +352,18 @@ bool UAVTalk::processInputByte(quint8 rxbyte) } // Check the lengths match - if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { // packet error - mismatched packet size + if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { + // packet error - mismatched packet size stats.rxErrors++; rxState = STATE_SYNC; UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)"); break; } - // Check if this is a single instance object (i.e. if the instance ID field is coming next) - if (rxObj == NULL) { - // This is a non-existing object, just skip to checksum - // and we'll send a NACK next. - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)"); - rxInstId = 0; - rxCount = 0; - } else if (rxObj->isSingleInstance()) { - // If there is a payload get it, otherwise receive checksum - if (rxLength > 0) { - rxState = STATE_DATA; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)"); - } else { - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum"); - } - rxInstId = 0; - rxCount = 0; - } else { - rxState = STATE_INSTID; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID"); - rxCount = 0; - } + rxCount = 0; + rxInstId = 0; + rxState = STATE_INSTID; } - break; case STATE_INSTID: @@ -428,14 +412,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // The CRC byte rxCSPacket = rxbyte; - if (rxCS != rxCSPacket) { // packet error - faulty CRC + if (rxCS != rxCSPacket) { + // packet error - faulty CRC stats.rxErrors++; rxState = STATE_SYNC; UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)"); break; } - if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size + if (rxPacketLength != packetSize + 1) { + // packet error - mismatched packet size stats.rxErrors++; rxState = STATE_SYNC; UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)"); @@ -443,12 +429,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte) } mutex->lock(); + receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); + if (useUDPMirror) { udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); } + stats.rxObjectBytes += rxLength; stats.rxObjects++; + mutex->unlock(); rxState = STATE_SYNC; @@ -456,6 +446,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) break; default: + rxState = STATE_SYNC; stats.rxErrors++; UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered. @@ -488,8 +479,13 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); - // Check if an ack is pending +#ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif if (obj != NULL) { + // Check if an ACK is pending + // TODO is it necessary to do that check? + // TODO if yes, why is the same check not done for OBJ_ACK below? updateAck(obj); } else { error = true; @@ -503,40 +499,55 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); - // Transmit ACK +#ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif if (obj != NULL) { - transmitObject(obj, TYPE_ACK, false); + // Object updated or created, transmit ACK + transmitObject(TYPE_ACK, objId, instId, obj); } else { error = true; } } else { error = true; } + if (error) { + // failed to update object, transmit NACK + transmitObject(TYPE_NACK, objId, instId, NULL); + } break; case TYPE_OBJ_REQ: - // Get object, if all instances are requested get instance 0 of the object + // Check if requested object exists if (allInstances) { + // All instances, so get instance zero obj = objMngr->getObject(objId); } else { obj = objMngr->getObject(objId, instId); } - // If object was found transmit it +#ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif if (obj != NULL) { - transmitObject(obj, TYPE_OBJ, allInstances); + // Object found, transmit it + transmitObject(TYPE_OBJ, objId, instId, obj); } else { - // Object was not found, transmit a NACK with the - // objId which was not found. - transmitNack(objId); error = true; } + if (error) { + // failed to send object, transmit NACK + transmitObject(TYPE_NACK, objId, instId, NULL); + } break; case TYPE_NACK: // All instances, not allowed for NACK messages if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); - // Check if object exists: +#ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif if (obj != NULL) { + // Check if a NACK is pending updateNack(obj); } else { error = true; @@ -548,8 +559,11 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); - // Check if an ack is pending +#ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif if (obj != NULL) { + // Check if an ACK is pending updateAck(obj); } else { error = true; @@ -559,6 +573,9 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * default: error = true; } + if (error) { + qWarning() << "UAVTalk - !!! error receiving object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + } // Done return !error; } @@ -608,65 +625,70 @@ void UAVTalk::updateNack(UAVObject *obj) if (!obj) { return; } - quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) { - transMap.remove(objId); - delete itr.value(); + Transaction *trans = findTransaction(obj); + // TODO handle allInstances + if (trans != NULL /* || itr.value()->allInstances)*/) { + closeTransaction(trans); + delete trans; emit transactionCompleted(obj, false); } } - /** * Check if a transaction is pending and if yes complete it. */ void UAVTalk::updateAck(UAVObject *obj) { - quint32 objId = obj->getObjID(); - - QMap::iterator itr = transMap.find(objId); - if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) { - transMap.remove(objId); - delete itr.value(); + Q_ASSERT(obj); + if (!obj) { + return; + } + Transaction *trans = findTransaction(obj); + // TODO handle allInstances + if (trans != NULL /* || itr.value()->allInstances)*/) { + closeTransaction(trans); + delete trans; emit transactionCompleted(obj, true); } } - /** * Send an object through the telemetry link. - * \param[in] obj Object to send * \param[in] type Transaction type - * \param[in] allInstances True is all instances of the object are to be sent + * \param[in] objId Object ID to send + * \param[in] instId Instance ID to send + * \param[in] obj Object to send (null when type is NACK) * \return Success (true), Failure (false) */ -bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances) +bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { + // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences. + // If all instances are requested on a single instance object it is an error - if (allInstances && obj->isSingleInstance()) { - allInstances = false; + if ((obj != NULL) && (instId == ALL_INSTANCES) && obj->isSingleInstance()) { + instId = 0; } + bool allInstances = (instId == ALL_INSTANCES); // Process message type if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { if (allInstances) { // Get number of instances - quint32 numInst = objMngr->getNumInstances(obj->getObjID()); + quint32 numInst = objMngr->getNumInstances(objId); // Send all instances - for (quint32 instId = 0; instId < numInst; ++instId) { - UAVObject *inst = objMngr->getObject(obj->getObjID(), instId); - transmitSingleObject(inst, type, false); + for (quint32 n = 0; n < numInst; ++n) { + UAVObject *o = objMngr->getObject(objId, n); + transmitSingleObject(type, objId, n, o); } return true; } else { - return transmitSingleObject(obj, type, false); + return transmitSingleObject(type, objId, instId, obj); } } else if (type == TYPE_OBJ_REQ) { - return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances); - } else if (type == TYPE_ACK) { + return transmitSingleObject(TYPE_OBJ_REQ, objId, instId, obj); + } else if (type == TYPE_ACK || type == TYPE_NACK) { if (!allInstances) { - return transmitSingleObject(obj, TYPE_ACK, false); + return transmitSingleObject(type, objId, instId, obj); } else { return false; } @@ -675,78 +697,35 @@ bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances) } } -/** - * Transmit a NACK through the telemetry link. - * \param[in] objId the ObjectID we rejected - */ -bool UAVTalk::transmitNack(quint32 objId) -{ - int dataOffset = 8; - - txBuffer[0] = SYNC_VAL; - txBuffer[1] = TYPE_NACK; - qToLittleEndian(objId, &txBuffer[4]); - - // Calculate checksum - txBuffer[dataOffset] = updateCRC(0, txBuffer, dataOffset); - - qToLittleEndian(dataOffset, &txBuffer[2]); - - // Send buffer, check that the transmit backlog does not grow above limit - if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) { - io->write((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH); - if (useUDPMirror) { - udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); - } - } else { - ++stats.txErrors; - return false; - } - - // Update stats - stats.txBytes += 8 + CHECKSUM_LENGTH; - - // Done - return true; -} - - /** * Send an object through the telemetry link. - * \param[in] obj Object handle to send * \param[in] type Transaction type + * \param[in] objId Object ID to send + * \param[in] instId Instance ID to send + * \param[in] obj Object to send (null when type is NACK) * \return Success (true), Failure (false) */ -bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances) +bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { +#ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif qint32 length; qint32 dataOffset; - quint32 objId; - quint16 instId; - quint16 allInstId = ALL_INSTANCES; // Setup type and object id fields - objId = obj->getObjID(); txBuffer[0] = SYNC_VAL; txBuffer[1] = type; + // data length inserted here below qToLittleEndian(objId, &txBuffer[4]); + dataOffset = 8; - // Setup instance ID if one is required - if (obj->isSingleInstance()) { - dataOffset = 8; - } else { - // Check if all instances are requested - if (allInstances) { - qToLittleEndian(allInstId, &txBuffer[8]); - } else { - instId = obj->getInstID(); - qToLittleEndian(instId, &txBuffer[8]); - } - dataOffset = 10; - } + // The instance ID is always sent + qToLittleEndian(instId, &txBuffer[8]); + dataOffset += 2; // Determine data length - if (type == TYPE_OBJ_REQ || type == TYPE_ACK) { + if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) { length = 0; } else { length = obj->getNumBytes(); @@ -811,6 +790,7 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data) { return crc_table[crc ^ data]; } + quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length) { while (length--) { @@ -818,3 +798,48 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length) } return crc; } + +UAVTalk::Transaction *UAVTalk::findTransaction(UAVObject *obj) +{ + // Lookup the transaction in the transaction map + QMap *objTransactions = transMap.value(obj->getObjID()); + if (objTransactions != NULL) { + return objTransactions->value(obj->getInstID()); + } + return NULL; +} + +void UAVTalk::openTransaction(Transaction *trans) +{ + QMap *objTransactions = transMap.value(trans->obj->getObjID()); + if (objTransactions == NULL) { + objTransactions = new QMap(); + transMap.insert(trans->obj->getObjID(), objTransactions); + } + objTransactions->insert(trans->obj->getInstID(), trans); +} + +void UAVTalk::closeTransaction(Transaction *trans) +{ + QMap *objTransactions = transMap.value(trans->obj->getObjID()); + if (objTransactions != NULL) { + objTransactions->remove(trans->obj->getInstID()); + // Keep the map even if it is empty + // There are at most 100 different object IDs... + } +} + +void UAVTalk::closeAllTransactions() +{ + foreach(quint32 objId, transMap.keys()) { + QMap *objTransactions = transMap.value(objId); + foreach(quint32 instId, objTransactions->keys()) { + Transaction *trans = objTransactions->value(instId); + qWarning() << "UAVTalk - closing active transaction for object" << trans->obj->toStringBrief(); + objTransactions->remove(instId); + delete trans; + } + transMap.remove(objId); + delete objTransactions; + } +} diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index fd22b66ef..c112a008c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -93,7 +93,6 @@ private: static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); static const quint16 ALL_INSTANCES = 0xFFFF; - static const quint16 OBJID_NOTFOUND = 0x0000; static const int TX_BUFFER_SIZE = 2 * 1024; static const quint8 crc_table[256]; @@ -105,7 +104,7 @@ private: QPointer io; UAVObjectManager *objMngr; QMutex *mutex; - QMap transMap; + QMap *> transMap; quint8 rxBuffer[MAX_PACKET_LENGTH]; quint8 txBuffer[MAX_PACKET_LENGTH]; // Variables used by the receive state machine @@ -129,17 +128,21 @@ private: QByteArray rxDataArray; // Methods - bool objectTransaction(UAVObject *obj, quint8 type, bool allInstances); + bool objectTransaction(quint8 type, UAVObject *obj, bool allInstances); bool processInputByte(quint8 rxbyte); bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length); UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data); void updateAck(UAVObject *obj); void updateNack(UAVObject *obj); - bool transmitNack(quint32 objId); - bool transmitObject(UAVObject *obj, quint8 type, bool allInstances); - bool transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances); + bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); + bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); quint8 updateCRC(quint8 crc, const quint8 data); quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); + + Transaction *findTransaction(UAVObject *obj); + void openTransaction(Transaction *trans); + void closeTransaction(Transaction *trans); + void closeAllTransactions(); }; #endif // UAVTALK_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro index 731cdf0ed..59275fa3c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro @@ -1,18 +1,29 @@ -QT += network TEMPLATE = lib TARGET = UAVTalk + +QT += network + +DEFINES += UAVTALK_LIBRARY + +#DEFINES += VERBOSE_TELEMETRY +#DEFINES += VERBOSE_UAVTALK + include(../../openpilotgcsplugin.pri) include(uavtalk_dependencies.pri) -HEADERS += uavtalk.h \ + +HEADERS += \ + uavtalk.h \ uavtalkplugin.h \ telemetrymonitor.h \ telemetrymanager.h \ uavtalk_global.h \ telemetry.h -SOURCES += uavtalk.cpp \ + +SOURCES += \ + uavtalk.cpp \ uavtalkplugin.cpp \ telemetrymonitor.cpp \ telemetrymanager.cpp \ telemetry.cpp -DEFINES += UAVTALK_LIBRARY + OTHER_FILES += UAVTalk.pluginspec From 50a0a4f512a4b3646c41de175c3b9a9b35cd37ec Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 28 Nov 2013 23:42:08 +0100 Subject: [PATCH 028/113] OP-1122 OP-1125 uavtalk - minor OPReview-593 related cleanups --- flight/uavtalk/inc/uavtalk_priv.h | 1 - flight/uavtalk/uavtalk.c | 48 +++++++++---------- .../src/plugins/uavtalk/uavtalk.cpp | 40 +++++++++------- .../src/plugins/uavtalk/uavtalk.h | 3 +- 4 files changed, 50 insertions(+), 42 deletions(-) diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 310f9e351..4b3393c9d 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -63,7 +63,6 @@ typedef struct { uint32_t objId; uint16_t instId; uint32_t length; - uint8_t instanceLength; uint8_t timestampLength; uint8_t cs; uint16_t timestamp; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 1dcfd8242..ed184a18b 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -32,6 +32,8 @@ #include "openpilot.h" #include "uavtalk_priv.h" +// Size of instance ID (2 bytes) +#define UAVTALK_INSTANCE_LENGTH 2 // Private functions static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout); @@ -422,23 +424,23 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; } - // Message always contain an instance ID - iproc->instanceLength = 2; - // Check length and determine next state if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + // packet error - exceeded payload max length connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } // Check the lengths match - if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size + if ((iproc->rxPacketLength + UAVTALK_INSTANCE_LENGTH + iproc->timestampLength + iproc->length) != iproc->packet_size) { + // packet error - mismatched packet size connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } + // Message always contain an instance ID iproc->rxCount = 0; iproc->instId = 0; iproc->state = UAVTALK_STATE_INSTID; @@ -585,25 +587,23 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // Lock xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY); - // Setup type and object id fields - outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + // Setup sync byte + outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; + // Setup type outConnection->txBuffer[1] = inIproc->type; - // data length inserted here below - int32_t dataOffset = 8; + // next 2 bytes are reserved for data length (inserted here later) + int32_t dataOffset = 4; if (inIproc->objId != 0) { + // Setup object ID outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF); outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF); outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF); outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF); - // Setup instance ID if one is required - if (inIproc->instanceLength > 0) { - outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); - outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); - dataOffset = 10; - } - } else { - dataOffset = 4; + // Setup instance ID + outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); + outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); + dataOffset = 10; } // Add timestamp when the transaction type is appropriate @@ -860,26 +860,26 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, int32_t length; int32_t dataOffset; - // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences. + // IMPORTANT : obj can be null (when type is NACK for example) if (!connection->outStream) { return -1; } - // Setup type and object id fields - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + // Setup sync byte + connection->txBuffer[0] = UAVTALK_SYNC_VAL; + // Setup type connection->txBuffer[1] = type; - // data length inserted here below + // next 2 bytes are reserved for data length (inserted here later) + // Setup object ID connection->txBuffer[4] = (uint8_t)(objId & 0xFF); connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - dataOffset = 8; - - // The instance ID is always sent + // Setup instance ID connection->txBuffer[8] = (uint8_t)(instId & 0xFF); connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset += 2; + dataOffset = 10; // Add timestamp when the transaction type is appropriate if (type & UAVTALK_TIMESTAMPED) { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 9df149d34..f10f58dc0 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -248,6 +248,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_SYNC: if (rxbyte != SYNC_VAL) { + // continue until synch byte is matched UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")"); break; } @@ -337,14 +338,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) { rxLength = 0; } else { - rxLength = rxObj->getNumBytes(); + if (rxObj) { + rxLength = rxObj->getNumBytes(); + } else { + rxLength = packetSize - rxPacketLength; + } } - // The instance ID is always sent - rxInstanceLength = 2; - // Check length and determine next state if (rxLength >= MAX_PAYLOAD_LENGTH) { + // packet error - exceeded payload max length stats.rxErrors++; rxState = STATE_SYNC; UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); @@ -352,7 +355,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) } // Check the lengths match - if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { + if ((rxPacketLength + INSTANCE_LENGTH + rxLength) != packetSize) { // packet error - mismatched packet size stats.rxErrors++; rxState = STATE_SYNC; @@ -360,6 +363,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) break; } + // Message always contain an instance ID rxCount = 0; rxInstId = 0; rxState = STATE_INSTID; @@ -707,22 +711,25 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje */ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { -#ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); -#endif qint32 length; qint32 dataOffset; - // Setup type and object id fields - txBuffer[0] = SYNC_VAL; - txBuffer[1] = type; - // data length inserted here below - qToLittleEndian(objId, &txBuffer[4]); - dataOffset = 8; + #ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif - // The instance ID is always sent + // IMPORTANT : obj can be null (when type is NACK for example) + + // Setup sync byte + txBuffer[0] = SYNC_VAL; + // Setup type + txBuffer[1] = type; + // next 2 bytes are reserved for data length (inserted here later) + // Setup object ID + qToLittleEndian(objId, &txBuffer[4]); + // Setup instance ID qToLittleEndian(instId, &txBuffer[8]); - dataOffset += 2; + dataOffset = 10; // Determine data length if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) { @@ -743,6 +750,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U } } + // Store the packet length qToLittleEndian(dataOffset + length, &txBuffer[2]); // Calculate checksum diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index c112a008c..8b27fa306 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -94,6 +94,8 @@ private: static const quint16 ALL_INSTANCES = 0xFFFF; + static const quint8 INSTANCE_LENGTH = 2; + static const int TX_BUFFER_SIZE = 2 * 1024; static const quint8 crc_table[256]; @@ -114,7 +116,6 @@ private: quint16 rxInstId; quint16 rxLength; quint16 rxPacketLength; - quint8 rxInstanceLength; quint8 rxCSPacket, rxCS; qint32 rxCount; From 15c4392ed5d99a9fa4df2a5da24d1b6b1c4ed882 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 29 Nov 2013 01:21:45 +0100 Subject: [PATCH 029/113] OP-1122 OP-1125 uavtalk - more OPReview-593 related cleanups --- flight/uavtalk/uavtalk.c | 15 ++++++++++++--- .../openpilotgcs/src/plugins/uavtalk/uavtalk.cpp | 7 +++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index ed184a18b..d39f55720 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -671,11 +671,19 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle, connection, return 0); + return connection->iproc.objId; } /** * Receive an object. This function process objects received through the telemetry stream. + * + * Parser errors are considered as transmission errors and are not NACKed. + * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack. + * + * Object handling errors are considered as application errors and are NACked. + * In that case we want to nack as there is no point in the sender retrying to send invalid objects. + * * \param[in] connection UAVTalkConnection to be used * \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK) * \param[in] objId ID of the object to work on @@ -714,9 +722,9 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! if (UAVObjUnpack(obj, instId, data)) { - // Check if an ack is pending OBJ_ACK below - // TODO is it necessary to do that check? - // TODO if yes, why is the same check not done for OBJ_ACK below? + // Check if this object acks a pending OBJ_REQ message + // any OBJ message can ack a pending OBJ_REQ message + // even one that was not sent in response to the OBJ_REQ message updateAck(connection, obj, instId); } else { @@ -750,6 +758,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // Check if requested object exists if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) { // Object found, transmit it + // This OBJ message will also ack this request sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj); } else { ret = -1; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index f10f58dc0..3711a5cdf 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -462,6 +462,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) /** * Receive an object. This function process objects received through the telemetry stream. + * + * Parser errors are considered as transmission errors and are not NACKed. + * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack. + * + * Object handling errors are considered as application errors and are NACked. + * In that case we want to nack as there is no point in the sender retrying to send invalid objects. + * * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK) * \param[in] obj Handle of the received object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. From 390c88b92a3b609f01aa44f3513cfd42a05ef6b0 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 29 Nov 2013 02:51:46 +0100 Subject: [PATCH 030/113] OP-1122 OP-1125 uavtalk - object request messages are now acked only by object messages and acked object messages are acked only by ack messages --- flight/uavtalk/inc/uavtalk_priv.h | 3 +- flight/uavtalk/uavtalk.c | 17 ++-- .../src/plugins/uavtalk/telemetry.cpp | 4 +- .../src/plugins/uavtalk/telemetry.h | 2 +- .../src/plugins/uavtalk/uavtalk.cpp | 84 +++++++++++-------- .../src/plugins/uavtalk/uavtalk.h | 15 ++-- 6 files changed, 71 insertions(+), 54 deletions(-) diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 4b3393c9d..a5b148440 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -77,7 +77,8 @@ typedef struct { xSemaphoreHandle lock; xSemaphoreHandle transLock; xSemaphoreHandle respSema; - UAVObjHandle respObj; + uint8_t respType; + uint32_t respObjId; uint16_t respInstId; UAVTalkStats stats; UAVTalkInputProcessor iproc; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index d39f55720..69d0eabcf 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -40,7 +40,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length); -static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId); +static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId); /** * Initialize the UAVTalk library @@ -289,7 +289,9 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); // Send object xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - connection->respObj = obj; + // expected response type + connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK; + connection->respObjId = UAVObjGetID(obj); connection->respInstId = instId; sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); @@ -300,7 +302,6 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type // Cancel transaction xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) - connection->respObj = 0; xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->transLock); return -1; @@ -725,7 +726,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // Check if this object acks a pending OBJ_REQ message // any OBJ message can ack a pending OBJ_REQ message // even one that was not sent in response to the OBJ_REQ message - updateAck(connection, obj, instId); + updateAck(connection, type, objId, instId); } else { ret = -1; @@ -770,12 +771,13 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, break; case UAVTALK_TYPE_NACK: // Do nothing on flight side, let it time out. + // Why? break; case UAVTALK_TYPE_ACK: // All instances, not allowed for ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Check if an ACK is pending - updateAck(connection, obj, instId); + updateAck(connection, type, objId, instId); } else { ret = -1; } @@ -797,11 +799,10 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. */ -static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId) +static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId) { - if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { + if (connection->respType == type && connection->respObjId == objId && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { xSemaphoreGive(connection->respSema); - connection->respObj = 0; } } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index f8674acff..dcb9464c8 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -374,7 +374,7 @@ void Telemetry::processObjectQueue() } transInfo->telem = this; // Insert the transaction into the transaction map. - openTransaction(transInfo); + startTransaction(transInfo); processObjectTransaction(transInfo); } @@ -542,7 +542,7 @@ ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) return NULL; } -void Telemetry::openTransaction(ObjectTransactionInfo *trans) +void Telemetry::startTransaction(ObjectTransactionInfo *trans) { QMap *objTransactions = transMap.value(trans->obj->getObjID()); if (objTransactions == NULL) { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 4462ec828..6f4f5ed32 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h @@ -137,7 +137,7 @@ private: void processObjectQueue(); ObjectTransactionInfo *findTransaction(UAVObject *obj); - void openTransaction(ObjectTransactionInfo *trans); + void startTransaction(ObjectTransactionInfo *trans); void closeTransaction(ObjectTransactionInfo *trans); void closeAllTransactions(); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 3711a5cdf..a349c0a8d 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -155,7 +155,15 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) { QMutexLocker locker(mutex); - return objectTransaction(TYPE_OBJ_REQ, obj, allInstances); + quint16 instId = 0; + + if (allInstances) { + instId = ALL_INSTANCES; + } + else if (obj) { + instId = obj->getInstID(); + } + return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj); } /** @@ -169,10 +177,18 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) { QMutexLocker locker(mutex); + quint16 instId = 0; + + if (allInstances) { + instId = ALL_INSTANCES; + } + else if (obj) { + instId = obj->getInstID(); + } if (acked) { - return objectTransaction(TYPE_OBJ_ACK, obj, allInstances); + return objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj); } else { - return objectTransaction(TYPE_OBJ, obj, allInstances); + return objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj); } } @@ -186,7 +202,7 @@ void UAVTalk::cancelTransaction(UAVObject *obj) if (io.isNull()) { return; } - Transaction *trans = findTransaction(obj); + Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID()); if (trans != NULL) { closeTransaction(trans); delete trans; @@ -203,24 +219,24 @@ void UAVTalk::cancelTransaction(UAVObject *obj) * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::objectTransaction(quint8 type, UAVObject *obj, bool allInstances) +bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { Q_ASSERT(obj); // Send object depending on if a response is needed // transactions of TYPE_OBJ_REQ are acked by the response - quint16 instId = allInstances ? ALL_INSTANCES : obj->getInstID(); if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { - if (transmitObject(type, obj->getObjID(), instId, obj)) { + if (transmitObject(type, objId, instId, obj)) { Transaction *trans = new Transaction(); - trans->obj = obj; - trans->allInstances = allInstances; - openTransaction(trans); + trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK; + trans->respObjId = objId; + trans->respInstId = instId; + startTransaction(trans); return true; } else { return false; } } else if (type == TYPE_OBJ) { - return transmitObject(type, obj->getObjID(), instId, obj); + return transmitObject(type, objId, instId, obj); } else { return false; } @@ -494,10 +510,10 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif if (obj != NULL) { - // Check if an ACK is pending - // TODO is it necessary to do that check? - // TODO if yes, why is the same check not done for OBJ_ACK below? - updateAck(obj); + // Check if this object acks a pending OBJ_REQ message + // any OBJ message can ack a pending OBJ_REQ message + // even one that was not sent in response to the OBJ_REQ message + updateAck(type, objId, instId, obj); } else { error = true; } @@ -559,7 +575,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * #endif if (obj != NULL) { // Check if a NACK is pending - updateNack(obj); + updateNack(type, objId, instId, obj); } else { error = true; } @@ -575,7 +591,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * #endif if (obj != NULL) { // Check if an ACK is pending - updateAck(obj); + updateAck(type, objId, instId, obj); } else { error = true; } @@ -630,15 +646,14 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data) /** * Check if a transaction is pending and if yes complete it. */ -void UAVTalk::updateNack(UAVObject *obj) +void UAVTalk::updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { Q_ASSERT(obj); if (!obj) { return; } - Transaction *trans = findTransaction(obj); - // TODO handle allInstances - if (trans != NULL /* || itr.value()->allInstances)*/) { + Transaction *trans = findTransaction(objId, instId); + if (trans) { closeTransaction(trans); delete trans; emit transactionCompleted(obj, false); @@ -648,15 +663,14 @@ void UAVTalk::updateNack(UAVObject *obj) /** * Check if a transaction is pending and if yes complete it. */ -void UAVTalk::updateAck(UAVObject *obj) +void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { Q_ASSERT(obj); if (!obj) { return; } - Transaction *trans = findTransaction(obj); - // TODO handle allInstances - if (trans != NULL /* || itr.value()->allInstances)*/) { + Transaction *trans = findTransaction(objId, instId); + if (trans && trans->respType == type) { closeTransaction(trans); delete trans; emit transactionCompleted(obj, true); @@ -814,31 +828,31 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length) return crc; } -UAVTalk::Transaction *UAVTalk::findTransaction(UAVObject *obj) +UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId) { // Lookup the transaction in the transaction map - QMap *objTransactions = transMap.value(obj->getObjID()); + QMap *objTransactions = transMap.value(objId); if (objTransactions != NULL) { - return objTransactions->value(obj->getInstID()); + return objTransactions->value(instId); } return NULL; } -void UAVTalk::openTransaction(Transaction *trans) +void UAVTalk::startTransaction(Transaction *trans) { - QMap *objTransactions = transMap.value(trans->obj->getObjID()); + QMap *objTransactions = transMap.value(trans->respObjId); if (objTransactions == NULL) { objTransactions = new QMap(); - transMap.insert(trans->obj->getObjID(), objTransactions); + transMap.insert(trans->respObjId, objTransactions); } - objTransactions->insert(trans->obj->getInstID(), trans); + objTransactions->insert(trans->respInstId, trans); } void UAVTalk::closeTransaction(Transaction *trans) { - QMap *objTransactions = transMap.value(trans->obj->getObjID()); + QMap *objTransactions = transMap.value(trans->respObjId); if (objTransactions != NULL) { - objTransactions->remove(trans->obj->getInstID()); + objTransactions->remove(trans->respInstId); // Keep the map even if it is empty // There are at most 100 different object IDs... } @@ -850,7 +864,7 @@ void UAVTalk::closeAllTransactions() QMap *objTransactions = transMap.value(objId); foreach(quint32 instId, objTransactions->keys()) { Transaction *trans = objTransactions->value(instId); - qWarning() << "UAVTalk - closing active transaction for object" << trans->obj->toStringBrief(); + qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId; objTransactions->remove(instId); delete trans; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 8b27fa306..c58732d0a 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -70,8 +70,9 @@ private slots: private: typedef struct { - UAVObject *obj; - bool allInstances; + quint8 respType; + quint32 respObjId; + quint16 respInstId; } Transaction; // Constants @@ -129,19 +130,19 @@ private: QByteArray rxDataArray; // Methods - bool objectTransaction(quint8 type, UAVObject *obj, bool allInstances); + bool objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); bool processInputByte(quint8 rxbyte); bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length); UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data); - void updateAck(UAVObject *obj); - void updateNack(UAVObject *obj); + void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); + void updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); quint8 updateCRC(quint8 crc, const quint8 data); quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); - Transaction *findTransaction(UAVObject *obj); - void openTransaction(Transaction *trans); + Transaction *findTransaction(quint32 objId, quint16 instId); + void startTransaction(Transaction *trans); void closeTransaction(Transaction *trans); void closeAllTransactions(); }; From 1a25e87fde8e5bbea278c4e217dcf49e31821ca6 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 29 Nov 2013 09:14:29 +0100 Subject: [PATCH 031/113] OP-1122 removed debug spurious debug logging when using waypoint editor + fixed compilation warnings --- .../src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp | 5 ----- ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp | 8 ++++++++ ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp | 1 - 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index c0dc61fdf..c90670475 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -292,26 +292,21 @@ void WayPointItem::setRelativeCoord(distBearingAltitude value) void WayPointItem::SetCoord(const internals::PointLatLng &value) { - qDebug() << "1 SetCoord(" << value.Lat() << "," << value.Lng() << ")" << "OLD:" << Coord().Lat() << "," << Coord().Lng(); if (qAbs(Coord().Lat() - value.Lat()) < 0.0001 && qAbs(Coord().Lng() - value.Lng()) < 0.0001) { - qDebug() << "2 SetCoord nothing changed returning"; return; } - qDebug() << "3 setCoord there were changes"; coord = value; distBearingAltitude back = relativeCoord; if (myHome) { map->Projection()->offSetFromLatLngs(myHome->Coord(), Coord(), back.distance, back.bearing); } if (qAbs(back.bearing - relativeCoord.bearing) > 0.01 || qAbs(back.distance - relativeCoord.distance) > 0.1) { - qDebug() << "4 setCoord-relative coordinates where also updated"; relativeCoord = back; } emit WPValuesChanged(this); RefreshPos(); RefreshToolTip(); this->update(); - qDebug() << "5 setCoord EXIT"; } void WayPointItem::SetDescription(const QString &value) { diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index 9fb0787d5..27e085e93 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -191,6 +191,7 @@ bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const } return false; } + QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const { switch (index) { @@ -285,7 +286,9 @@ QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int in case LOCKED: return row->locked; } + return NULL; } + QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const { if (role == Qt::DisplayRole) { @@ -394,7 +397,9 @@ QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, i } else { return QAbstractTableModel::headerData(section, orientation, role); } + return NULL; } + bool flightDataModel::setData(const QModelIndex &index, const QVariant &value, int role) { if (role == Qt::EditRole) { @@ -465,6 +470,7 @@ bool flightDataModel::insertRows(int row, int count, const QModelIndex & /*paren dataStorage.insert(row, data); } endInsertRows(); + return true; } bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*parent*/) @@ -478,6 +484,7 @@ bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*paren dataStorage.removeAt(row); } endRemoveRows(); + return true; } bool flightDataModel::writeToFile(QString fileName) @@ -617,6 +624,7 @@ bool flightDataModel::writeToFile(QString fileName) file.close(); return true; } + void flightDataModel::readFromFile(QString fileName) { // TODO warning message diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp index a6cd1b642..49fbca8b8 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp @@ -76,7 +76,6 @@ void MapDataDelegate::setEditorData(QWidget *editor, int value = index.model()->data(index, Qt::EditRole).toInt(); QComboBox *comboBox = static_cast(editor); int x = comboBox->findData(value); - qDebug() << "VALUE=" << x; comboBox->setCurrentIndex(x); } else { QItemDelegate::setEditorData(editor, index); From cb902900de0bf89f206f2b01af488732c3c5878e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 29 Nov 2013 09:23:27 +0100 Subject: [PATCH 032/113] OP-1122 OP-1125 uavtalk - fixed minor typos --- ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index a349c0a8d..35dee2083 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -264,7 +264,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_SYNC: if (rxbyte != SYNC_VAL) { - // continue until synch byte is matched + // continue until sync byte is matched UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")"); break; } @@ -735,7 +735,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U qint32 length; qint32 dataOffset; - #ifdef VERBOSE_UAVTALK +#ifdef VERBOSE_UAVTALK qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif From a6d0f09d21129742436dbece7ba188f5f1c588db Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 29 Nov 2013 19:40:10 +0100 Subject: [PATCH 033/113] OP-1122 OP-1125 minor change to uavobject::toStringBrief() method --- ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index cca8afb23..874ec4535 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -255,7 +255,7 @@ UAVObjectField *UAVObject::getField(const QString & name) } } // If this point is reached then the field was not found - qWarning() << "UAVObject::getField Non existant field " << name << " requested. This indicates a bug. Make sure you also have null checking for non-debug code."; + qWarning() << "UAVObject::getField Non existant field" << name << "requested. This indicates a bug. Make sure you also have null checking for non-debug code."; return NULL; } @@ -448,7 +448,7 @@ QString UAVObject::toStringBrief() { QString sout; - sout.append(QString("%1 (ID: %2:%3, %4bytes, %5)") + sout.append(QString("%1 (ID: %2-%3, %4 bytes, %5)") .arg(getName()) .arg(getObjID()) .arg(getInstID()) From 0dc334adea07a3fd516917ccc88439aa1dd4a5f2 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 1 Dec 2013 19:25:44 +0100 Subject: [PATCH 034/113] OP-1122 OP-1133 exposed send/request all instances of multi instance uav objects and made related uavtalk fixes --- flight/uavtalk/uavtalk.c | 28 +++- .../src/plugins/uavobjects/uavobject.cpp | 24 +++ .../src/plugins/uavobjects/uavobject.h | 6 +- .../src/plugins/uavtalk/telemetry.cpp | 147 +++++++++++------- .../src/plugins/uavtalk/telemetry.h | 11 +- .../src/plugins/uavtalk/uavtalk.cpp | 138 +++++++++------- .../src/plugins/uavtalk/uavtalk.h | 8 +- 7 files changed, 231 insertions(+), 131 deletions(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 69d0eabcf..95183d770 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -340,8 +340,10 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } if (iproc->rxPacketLength < 0xffff) { - iproc->rxPacketLength++; // update packet byte count + // update packet byte count + iproc->rxPacketLength++; } + // Receive state machine switch (iproc->state) { case UAVTALK_STATE_SYNC: @@ -445,7 +447,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->rxCount = 0; iproc->instId = 0; iproc->state = UAVTALK_STATE_INSTID; - break; case UAVTALK_STATE_INSTID: @@ -472,7 +473,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } else { iproc->state = UAVTALK_STATE_CS; } - break; case UAVTALK_STATE_TIMESTAMP: @@ -653,11 +653,14 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle, connection, return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; if (iproc->state != UAVTALK_STATE_COMPLETE) { return -1; } + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + return 0; } @@ -755,11 +758,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); } break; + case UAVTALK_TYPE_OBJ_REQ: // Check if requested object exists if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) { // Object found, transmit it - // This OBJ message will also ack this request + // The sent object will ack the object request on the receiver side sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj); } else { ret = -1; @@ -769,10 +773,18 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); } break; + case UAVTALK_TYPE_NACK: // Do nothing on flight side, let it time out. - // Why? + // TODO: + // The transaction takes the result code of the "semaphore taking operation" into account to determine success. + // If we give that semaphore in time, its "success" (ack received) + // If we do not give that semaphore before the timeout it will return failure. + // What would have to be done here is give the semaphore, but set a flag (for example connection->respFail=true) + // that indicates failure and then above where it checks for the result code, have it behave as if it failed + // if the explicit failure is set. break; + case UAVTALK_TYPE_ACK: // All instances, not allowed for ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { @@ -782,6 +794,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, ret = -1; } break; + default: ret = -1; } @@ -833,9 +846,10 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 if (instId == UAVOBJ_ALL_INSTANCES) { // Get number of instances numInst = UAVObjGetNumInstances(obj); - // Send all instances + // Send all instances in reverse order + // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) for (n = 0; n < numInst; ++n) { - sendSingleObject(connection, type, objId, n, obj); + sendSingleObject(connection, type, objId, numInst - n - 1, obj); } return 0; } else { diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 874ec4535..1adc76496 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -179,6 +179,17 @@ void UAVObject::requestUpdate() emit updateRequested(this); } +/** + * Request that all instances of this object are updated with the latest values from the autopilot + * Must be called on instance zero + */ +void UAVObject::requestUpdateAll() +{ + if (instID == 0) { + emit updateRequested(this, true); + } +} + /** * Signal that the object has been updated */ @@ -188,6 +199,19 @@ void UAVObject::updated() emit objectUpdated(this); } +/** + * Signal that all instance of the object have been updated + * Must be called on instance zero + */ +void UAVObject::updatedAll() +{ + if (instID == 0) { + emit objectUpdatedManual(this, true); + // TODO call objectUpdated() for all instances? + //emit objectUpdated(this); + } +} + /** * Lock mutex of this object */ diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h index 882378e7a..c5fdafa68 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h @@ -144,15 +144,17 @@ public: public slots: void requestUpdate(); + void requestUpdateAll(); void updated(); + void updatedAll(); signals: void objectUpdated(UAVObject *obj); void objectUpdatedAuto(UAVObject *obj); - void objectUpdatedManual(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj, bool all = false); void objectUpdatedPeriodic(UAVObject *obj); void objectUnpacked(UAVObject *obj); - void updateRequested(UAVObject *obj); + void updateRequested(UAVObject *obj, bool all = false); void transactionCompleted(UAVObject *obj, bool success); void newInstance(UAVObject *obj); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index dcb9464c8..7ddca5f47 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -36,28 +36,33 @@ /** * Constructor */ -Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) +Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMngr), utalk(utalk) { - this->utalk = utalk; - this->objMngr = objMngr; mutex = new QMutex(QMutex::Recursive); - // Process all objects in the list + + // Register all objects in the list QList< QList > objs = objMngr->getObjects(); for (int objidx = 0; objidx < objs.length(); ++objidx) { - registerObject(objs[objidx][0]); // we only need to register one instance per object type + // we only need to register one instance per object type + registerObject(objs[objidx][0]); } + // Listen to new object creations connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); + // Listen to transaction completions connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); + // Get GCS stats object gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); + // Setup and start the periodic timer timeToNextUpdateMs = 0; updateTimer = new QTimer(this); connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates())); updateTimer->start(1000); + // Setup and start the stats timer txErrors = 0; txRetries = 0; @@ -66,7 +71,6 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) Telemetry::~Telemetry() { closeAllTransactions(); - } /** @@ -121,27 +125,36 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs) */ void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask) { - // TODO why connect systematically to all instances? + // TODO why connect systematically to all instances? + // It is probably not needed to connect to always connect to all instances. QList objs = objMngr->getObjectInstances(obj->getObjID()); for (int n = 0; n < objs.length(); ++n) { - // Disconnect all - objs[n]->disconnect(this); - // Connect only the selected events - if ((eventMask & EV_UNPACKED) != 0) { - connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); - } - if ((eventMask & EV_UPDATED) != 0) { - connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); - } - if ((eventMask & EV_UPDATED_MANUAL) != 0) { - connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *))); - } - if ((eventMask & EV_UPDATED_PERIODIC) != 0) { - connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *))); - } - if ((eventMask & EV_UPDATE_REQ) != 0) { - connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *))); - } + connectToObject(objs[n], eventMask); + } +} + +/** + * Connect to all instances of an object depending on the event mask specified + */ +void Telemetry::connectToObject(UAVObject *obj, quint32 eventMask) +{ + // Disconnect all + obj->disconnect(this); + // Connect only the selected events + if ((eventMask & EV_UNPACKED) != 0) { + connect(obj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); + } + if ((eventMask & EV_UPDATED) != 0) { + connect(obj, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); + } + if ((eventMask & EV_UPDATED_MANUAL) != 0) { + connect(obj, SIGNAL(objectUpdatedManual(UAVObject *, bool)), this, SLOT(objectUpdatedManual(UAVObject *, bool))); + } + if ((eventMask & EV_UPDATED_PERIODIC) != 0) { + connect(obj, SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *))); + } + if ((eventMask & EV_UPDATE_REQ) != 0) { + connect(obj, SIGNAL(updateRequested(UAVObject *, bool)), this, SLOT(updateRequested(UAVObject *, bool))); } } @@ -160,19 +173,21 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType) if (updateMode == UAVObject::UPDATEMODE_PERIODIC) { // Set update period setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); - // Connect signals for all instances + // Connect signals eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; if (dynamic_cast(obj) != NULL) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + // we also need to act on remote updates (unpack events) + eventMask |= EV_UNPACKED; } connectToObjectInstances(obj, eventMask); } else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) { // Set update period setUpdatePeriod(obj, 0); - // Connect signals for all instances + // Connect signals eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (dynamic_cast(obj) != NULL) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + // we also need to act on remote updates (unpack events) + eventMask |= EV_UNPACKED; } connectToObjectInstances(obj, eventMask); } else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) { @@ -182,24 +197,26 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType) if (eventType == EV_NONE) { setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); } - // Connect signals for all instances + // Connect signals eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates - // Connect signals for all instances + // Connect signals eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; } if (dynamic_cast(obj) != NULL) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + // we also need to act on remote updates (unpack events) + eventMask |= EV_UNPACKED; } connectToObjectInstances(obj, eventMask); } else if (updateMode == UAVObject::UPDATEMODE_MANUAL) { // Set update period setUpdatePeriod(obj, 0); - // Connect signals for all instances + // Connect signals eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (dynamic_cast(obj) != NULL) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + // we also need to act on remote updates (unpack events) + eventMask |= EV_UNPACKED; } connectToObjectInstances(obj, eventMask); } @@ -242,7 +259,7 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) // Check if more retries are pending if (transInfo->retriesRemaining > 0) { #ifdef VERBOSE_TELEMETRY - qDebug() << QString("Telemetry - transaction timed out for object %1, retrying...").arg(transInfo->obj->toStringBrief()); + qDebug().nospace() << "Telemetry - transaction timed out for object " << transInfo->obj->toStringBrief() << ", retrying..."; #endif --transInfo->retriesRemaining; processObjectTransaction(transInfo); @@ -253,7 +270,7 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) // Terminate transaction utalk->cancelTransaction(transInfo->obj); // Send signal - qWarning() << QString("Telemetry - !!! transaction timed out for object %1").arg(transInfo->obj->toStringBrief()); + qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief(); transInfo->obj->emitTransactionCompleted(false); // Remove this transaction as it's complete. // FIXME : also remove transaction from UAVTalk transaction map @@ -273,12 +290,12 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) // Initiate transaction if (transInfo->objRequest) { #ifdef VERBOSE_TELEMETRY - qDebug() << QString("Telemetry - sending object request for %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : ""); + qDebug().nospace() << "Telemetry - sending object request for " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); #endif utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); } else { #ifdef VERBOSE_TELEMETRY - qDebug() << QString("Telemetry - sending object %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : ""); + qDebug().nospace() << "Telemetry - sending object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); #endif utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); } @@ -308,7 +325,7 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn objPriorityQueue.enqueue(objInfo); } else { ++txErrors; - qWarning() << QString("Telemetry - !!! priority event queue is full, event lost (%1)").arg(obj->toStringBrief()); + qWarning().nospace() << "Telemetry - !!! priority event queue is full, event lost " << obj->toStringBrief(); obj->emitTransactionCompleted(false); } } else { @@ -316,7 +333,7 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn objQueue.enqueue(objInfo); } else { ++txErrors; - qWarning() << QString("Telemetry - !!! event queue is full, event lost (%1)").arg(obj->toStringBrief()); + qWarning().nospace() << "Telemetry - !!! event queue is full, event lost " << obj->toStringBrief(); obj->emitTransactionCompleted(false); } } @@ -356,8 +373,13 @@ void Telemetry::processObjectQueue() UAVObject::Metadata metadata = objInfo.obj->getMetadata(); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) { + // Check if a transaction for that object already exists + // It is allowed to have multiple transaction on the same object ID provided that the instance IDs are different + // If an "all instances" transaction is running, then it is not allowed to start another transaction with same object ID + // If a single instance transaction is running, then starting an "all instance" transaction is not allowed + // TODO make the above logic a reality... if (findTransaction(objInfo.obj)) { - qWarning() << QString("Telemetry - !!! Making request for an object: %1 for which a request is already in progress").arg(objInfo.obj->toStringBrief()); + qWarning() << "Telemetry - !!! Making request for a object " << objInfo.obj->toStringBrief() << " for which a request is already in progress"; objInfo.obj->emitTransactionCompleted(false); return; } @@ -374,7 +396,7 @@ void Telemetry::processObjectQueue() } transInfo->telem = this; // Insert the transaction into the transaction map. - startTransaction(transInfo); + openTransaction(transInfo); processObjectTransaction(transInfo); } @@ -491,11 +513,12 @@ void Telemetry::objectUpdatedAuto(UAVObject *obj) processObjectUpdates(obj, EV_UPDATED, false, true); } -void Telemetry::objectUpdatedManual(UAVObject *obj) +void Telemetry::objectUpdatedManual(UAVObject *obj, bool all) { QMutexLocker locker(mutex); - processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true); + bool allInstances = obj->isSingleInstance() ? false : all; + processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true); } void Telemetry::objectUpdatedPeriodic(UAVObject *obj) @@ -512,10 +535,12 @@ void Telemetry::objectUnpacked(UAVObject *obj) processObjectUpdates(obj, EV_UNPACKED, false, true); } -void Telemetry::updateRequested(UAVObject *obj) +void Telemetry::updateRequested(UAVObject *obj, bool all) { QMutexLocker locker(mutex); - processObjectUpdates(obj, EV_UPDATE_REQ, false, true); + + bool allInstances = obj->isSingleInstance() ? false : all; + processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true); } void Telemetry::newObject(UAVObject *obj) @@ -534,29 +559,43 @@ void Telemetry::newInstance(UAVObject *obj) ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) { + quint32 objId = obj->getObjID(); + quint16 instId = obj->getInstID(); + // Lookup the transaction in the transaction map - QMap *objTransactions = transMap.value(obj->getObjID()); + QMap *objTransactions = transMap.value(objId); if (objTransactions != NULL) { - return objTransactions->value(obj->getInstID()); + ObjectTransactionInfo *trans = objTransactions->value(instId); + if (trans == NULL) { + // see if there is an ALL_INSTANCES transaction + trans = objTransactions->value(UAVTalk::ALL_INSTANCES); + } + return trans; } return NULL; } -void Telemetry::startTransaction(ObjectTransactionInfo *trans) +void Telemetry::openTransaction(ObjectTransactionInfo *trans) { - QMap *objTransactions = transMap.value(trans->obj->getObjID()); + quint32 objId = trans->obj->getObjID(); + quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID(); + + QMap *objTransactions = transMap.value(objId); if (objTransactions == NULL) { objTransactions = new QMap(); - transMap.insert(trans->obj->getObjID(), objTransactions); + transMap.insert(objId, objTransactions); } - objTransactions->insert(trans->obj->getInstID(), trans); + objTransactions->insert(instId, trans); } void Telemetry::closeTransaction(ObjectTransactionInfo *trans) { - QMap *objTransactions = transMap.value(trans->obj->getObjID()); + quint32 objId = trans->obj->getObjID(); + quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID(); + + QMap *objTransactions = transMap.value(objId); if (objTransactions != NULL) { - objTransactions->remove(trans->obj->getInstID()); + objTransactions->remove(instId); // Keep the map even if it is empty // There are at most 100 different object IDs... } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 6f4f5ed32..327a9d5d0 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h @@ -95,8 +95,8 @@ private: EV_UNPACKED = 0x01, /** Object data updated by unpacking */ EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ - EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */ - EV_UPDATE_REQ = 0x010 /** Request to update object data */ + EV_UPDATED_PERIODIC = 0x08, /** Object update event generated by timer */ + EV_UPDATE_REQ = 0x10 /** Request to update object data */ } EventMask; typedef struct { @@ -131,22 +131,23 @@ private: void addObject(UAVObject *obj); void setUpdatePeriod(UAVObject *obj, qint32 periodMs); void connectToObjectInstances(UAVObject *obj, quint32 eventMask); + void connectToObject(UAVObject *obj, quint32 eventMask); void updateObject(UAVObject *obj, quint32 eventMask); void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority); void processObjectTransaction(ObjectTransactionInfo *transInfo); void processObjectQueue(); ObjectTransactionInfo *findTransaction(UAVObject *obj); - void startTransaction(ObjectTransactionInfo *trans); + void openTransaction(ObjectTransactionInfo *trans); void closeTransaction(ObjectTransactionInfo *trans); void closeAllTransactions(); private slots: void objectUpdatedAuto(UAVObject *obj); - void objectUpdatedManual(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj, bool all = false); void objectUpdatedPeriodic(UAVObject *obj); void objectUnpacked(UAVObject *obj); - void updateRequested(UAVObject *obj); + void updateRequested(UAVObject *obj, bool all = false); void newObject(UAVObject *obj); void newInstance(UAVObject *obj); void processPeriodicUpdates(); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 35dee2083..bd2a7d1c7 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -29,6 +29,7 @@ #include #include #include + // #define UAVTALK_DEBUG #ifdef UAVTALK_DEBUG #define UAVTALK_QXTLOG_DEBUG(args ...) @@ -36,6 +37,8 @@ #define UAVTALK_QXTLOG_DEBUG(args ...) #endif // UAVTALK_DEBUG +//#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC) + #define SYNC_VAL 0x3C const quint8 UAVTalk::crc_table[256] = { @@ -205,7 +208,6 @@ void UAVTalk::cancelTransaction(UAVObject *obj) Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID()); if (trans != NULL) { closeTransaction(trans); - delete trans; } } @@ -226,11 +228,7 @@ bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVO // transactions of TYPE_OBJ_REQ are acked by the response if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { if (transmitObject(type, objId, instId, obj)) { - Transaction *trans = new Transaction(); - trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK; - trans->respObjId = objId; - trans->respInstId = instId; - startTransaction(trans); + openTransaction(type, objId, instId); return true; } else { return false; @@ -507,7 +505,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * // Get object and update its data obj = updateObject(objId, instId, data); #ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif if (obj != NULL) { // Check if this object acks a pending OBJ_REQ message @@ -521,13 +519,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * error = true; } break; + case TYPE_OBJ_ACK: // All instances, not allowed for OBJ_ACK messages if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); #ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif if (obj != NULL) { // Object updated or created, transmit ACK @@ -543,6 +542,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * transmitObject(TYPE_NACK, objId, instId, NULL); } break; + case TYPE_OBJ_REQ: // Check if requested object exists if (allInstances) { @@ -552,10 +552,11 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * obj = objMngr->getObject(objId, instId); } #ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif if (obj != NULL) { // Object found, transmit it + // The sent object will ack the object request on the receiver side transmitObject(TYPE_OBJ, objId, instId, obj); } else { error = true; @@ -565,29 +566,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * transmitObject(TYPE_NACK, objId, instId, NULL); } break; - case TYPE_NACK: - // All instances, not allowed for NACK messages - if (!allInstances) { - // Get object - obj = objMngr->getObject(objId, instId); -#ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); -#endif - if (obj != NULL) { - // Check if a NACK is pending - updateNack(type, objId, instId, obj); - } else { - error = true; - } - } - break; + case TYPE_ACK: // All instances, not allowed for ACK messages if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); #ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif if (obj != NULL) { // Check if an ACK is pending @@ -597,6 +583,24 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * } } break; + + case TYPE_NACK: + // All instances, not allowed for NACK messages + if (!allInstances) { + // Get object + obj = objMngr->getObject(objId, instId); +#ifdef VERBOSE_UAVTALK + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif + if (obj != NULL) { + // Check if a NACK is pending + updateNack(objId, instId, obj); + } else { + error = true; + } + } + break; + default: error = true; } @@ -620,22 +624,25 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data) // If the instance does not exist create it if (obj == NULL) { // Get the object type - UAVObject *tobj = objMngr->getObject(objId); - if (tobj == NULL) { + UAVObject *typeObj = objMngr->getObject(objId); + if (typeObj == NULL) { + qWarning() << "UAVTalk - Failed to get object, object ID :" << objId; return NULL; } // Make sure this is a data object - UAVDataObject *dobj = dynamic_cast(tobj); - if (dobj == NULL) { + UAVDataObject *dataObj = dynamic_cast(typeObj); + if (dataObj == NULL) { + qWarning() << "UAVTalk - Object is not a data object, object ID :" << objId; return NULL; } // Create a new instance, unpack and register - UAVDataObject *instobj = dobj->clone(instId); - if (!objMngr->registerObject(instobj)) { + UAVDataObject *instObj = dataObj->clone(instId); + if (!objMngr->registerObject(instObj)) { + qWarning() << "UAVTalk - Failed to register object " << instObj->toStringBrief(); return NULL; } - instobj->unpack(data); - return instobj; + instObj->unpack(data); + return instObj; } else { // Unpack data into object instance obj->unpack(data); @@ -643,23 +650,6 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data) } } -/** - * Check if a transaction is pending and if yes complete it. - */ -void UAVTalk::updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) -{ - Q_ASSERT(obj); - if (!obj) { - return; - } - Transaction *trans = findTransaction(objId, instId); - if (trans) { - closeTransaction(trans); - delete trans; - emit transactionCompleted(obj, false); - } -} - /** * Check if a transaction is pending and if yes complete it. */ @@ -671,9 +661,26 @@ void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *o } Transaction *trans = findTransaction(objId, instId); if (trans && trans->respType == type) { + if (trans->respInstId != ALL_INSTANCES || instId == 0) { + closeTransaction(trans); + emit transactionCompleted(obj, true); + } + } +} + +/** + * Check if a transaction is pending and if yes complete it. + */ +void UAVTalk::updateNack(quint32 objId, quint16 instId, UAVObject *obj) +{ + Q_ASSERT(obj); + if (!obj) { + return; + } + Transaction *trans = findTransaction(objId, instId); + if (trans) { closeTransaction(trans); - delete trans; - emit transactionCompleted(obj, true); + emit transactionCompleted(obj, false); } } @@ -700,10 +707,12 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje if (allInstances) { // Get number of instances quint32 numInst = objMngr->getNumInstances(objId); - // Send all instances + // Send all instances in reverse order + // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) for (quint32 n = 0; n < numInst; ++n) { - UAVObject *o = objMngr->getObject(objId, n); - transmitSingleObject(type, objId, n, o); + quint32 i = numInst - n - 1; + UAVObject *o = objMngr->getObject(objId, i); + transmitSingleObject(type, objId, i, o); } return true; } else { @@ -736,7 +745,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U qint32 dataOffset; #ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); #endif // IMPORTANT : obj can be null (when type is NACK for example) @@ -833,13 +842,23 @@ UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId) // Lookup the transaction in the transaction map QMap *objTransactions = transMap.value(objId); if (objTransactions != NULL) { - return objTransactions->value(instId); + Transaction *trans = objTransactions->value(instId); + if (trans == NULL) { + // see if there is an ALL_INSTANCES transaction + trans = objTransactions->value(ALL_INSTANCES); + } + return trans; } return NULL; } -void UAVTalk::startTransaction(Transaction *trans) +void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId) { + Transaction *trans = new Transaction(); + trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK; + trans->respObjId = objId; + trans->respInstId = instId; + QMap *objTransactions = transMap.value(trans->respObjId); if (objTransactions == NULL) { objTransactions = new QMap(); @@ -855,6 +874,7 @@ void UAVTalk::closeTransaction(Transaction *trans) objTransactions->remove(trans->respInstId); // Keep the map even if it is empty // There are at most 100 different object IDs... + delete trans; } } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index c58732d0a..e5be192f7 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -41,6 +41,8 @@ class UAVTALK_EXPORT UAVTalk : public QObject { Q_OBJECT public: + static const quint16 ALL_INSTANCES = 0xFFFF; + typedef struct { quint32 txBytes; quint32 rxBytes; @@ -93,8 +95,6 @@ private: static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); - static const quint16 ALL_INSTANCES = 0xFFFF; - static const quint8 INSTANCE_LENGTH = 2; static const int TX_BUFFER_SIZE = 2 * 1024; @@ -135,14 +135,14 @@ private: bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length); UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data); void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); - void updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); + void updateNack(quint32 objId, quint16 instId, UAVObject *obj); bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); quint8 updateCRC(quint8 crc, const quint8 data); quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); Transaction *findTransaction(quint32 objId, quint16 instId); - void startTransaction(Transaction *trans); + void openTransaction(quint8 type, quint32 objId, quint16 instId); void closeTransaction(Transaction *trans); void closeAllTransactions(); }; From 398a3ab0e6424376af9cf6e7132d11f98c6b707c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 1 Dec 2013 19:38:23 +0100 Subject: [PATCH 035/113] OP-112 OP-1120 waypoint editor now sends and requests Waypoints and PathActions in a transacted and asynchronous manner. Board does not send Waypoints and PathActions periodically anymore. --- .../src/plugins/opmap/modeluavoproxy.cpp | 115 ++++++++++++++---- .../src/plugins/opmap/modeluavoproxy.h | 14 ++- .../src/plugins/opmap/opmapgadgetwidget.cpp | 6 +- shared/uavobjectdefinition/pathaction.xml | 34 +++--- shared/uavobjectdefinition/waypoint.xml | 8 +- 5 files changed, 128 insertions(+), 49 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 9dbc1fbfb..6937ee1b0 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -31,14 +31,83 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Q_ASSERT(pm != NULL); + objManager = pm->getObject(); Q_ASSERT(objManager != NULL); - waypointObj = Waypoint::GetInstance(objManager); - Q_ASSERT(waypointObj != NULL); - pathactionObj = PathAction::GetInstance(objManager); - Q_ASSERT(pathactionObj != NULL); +} + +void ModelUavoProxy::sendFlightPlan() +{ + modelToObjects(); + + Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objManager, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + + completionCount = 0; + completionSuccessCount = 0; + + waypoint->updatedAll(); + action->updatedAll(); +} + +void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) +{ + obj->disconnect(this); + + completionCount++; + if (success) { + completionSuccessCount++; + } + + if (completionCount == 2) { + qDebug() << "ModelUavoProxy::flightPlanSent - success" << (completionSuccessCount == 2); + if (completionSuccessCount == 2) { + // TODO : popup? + } + else { + // TODO : popup? + } + } +} + +void ModelUavoProxy::receiveFlightPlan() +{ + Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objManager, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + + completionCount = 0; + completionSuccessCount = 0; + + waypoint->requestUpdateAll(); + action->requestUpdateAll(); +} + +void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) +{ + obj->disconnect(this); + + completionCount++; + if (success) { + completionSuccessCount++; + } + + if (completionCount == 2) { + qDebug() << "ModelUavoProxy::flightPlanReceived - success" << (completionSuccessCount == 2); + if (completionSuccessCount == 2) { + objectsToModel(); + // TODO : popup? + } + else { + // TODO : popup? + } + } } void ModelUavoProxy::modelToObjects() @@ -80,10 +149,8 @@ void ModelUavoProxy::modelToObjects() action = createPathAction(actionCount, action); actionCount++; - // send action update to UAV + // update UAVObject action->setData(actionData); - action->updated(); - qDebug() << "ModelUAVProxy::modelToObjects - sent action instance :" << action->getInstID(); } else { action->deleteLater(); @@ -104,16 +171,14 @@ void ModelUavoProxy::modelToObjects() // connect waypoint to path action waypointData.Action = action->getInstID(); - // send waypoint update to UAV + // update UAVObject waypoint->setData(waypointData); - waypoint->updated(); - qDebug() << "ModelUAVProxy::modelToObjects - sent waypoint instance :" << waypoint->getInstID(); } } Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { Waypoint *waypoint = NULL; - int count = objManager->getNumInstances(waypointObj->getObjID()); + int count = objManager->getNumInstances(Waypoint::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; @@ -138,7 +203,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *action = NULL; - int count = objManager->getNumInstances(pathactionObj->getObjID()); + int count = objManager->getNumInstances(PathAction::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; @@ -162,7 +227,7 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { } PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { - int instancesCount = objManager->getNumInstances(pathactionObj->getObjID()); + int instancesCount = objManager->getNumInstances(PathAction::OBJID); int count = actionCount <= instancesCount ? actionCount : instancesCount; for (int i = 0; i < count; ++i) { PathAction *action = PathAction::GetInstance(objManager, i); @@ -189,21 +254,29 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD void ModelUavoProxy::objectsToModel() { - myModel->removeRows(0, myModel->rowCount()); + int instanceCount = objManager->getNumInstances(Waypoint::OBJID); + // TODO retain only reachable waypoints + + int rowCount = myModel->rowCount(); + if (instanceCount < rowCount) { + myModel->removeRows(instanceCount, rowCount - instanceCount); + } + else if (instanceCount > rowCount) { + myModel->insertRows(rowCount, instanceCount - rowCount); + } - int instanceCount = objManager->getNumInstances(waypointObj->getObjID()); for (int i = 0; i < instanceCount; ++i) { Waypoint *waypoint = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypoint); if (!waypoint) { continue; } - myModel->insertRow(i); Waypoint::DataFields waypointData = waypoint->getData(); waypointToModel(i, waypointData); - PathAction *action = PathAction::GetInstance(objManager, waypointData.Action); + int actionId = waypointData.Action; + PathAction *action = PathAction::GetInstance(objManager, actionId); Q_ASSERT(action); if (!action) { continue; @@ -226,8 +299,8 @@ void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { index = myModel->index(i, flightDataModel::VELOCITY); velocity = myModel->data(index).toFloat(); - data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / (180 * M_PI)); - data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / (180 * M_PI)); + data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); + data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); data.Position[Waypoint::POSITION_DOWN] = -altitude; data.Velocity = velocity; } @@ -237,7 +310,7 @@ void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) { double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] + data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]); - double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * (180 / M_PI); + double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; if (bearing != bearing) { bearing = 0; } diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 38a88d1e8..1b685a739 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -38,15 +38,18 @@ class ModelUavoProxy : public QObject { public: explicit ModelUavoProxy(QObject *parent, flightDataModel *model); -public slots: void modelToObjects(); void objectsToModel(); +public slots: + void sendFlightPlan(); + void receiveFlightPlan(); + private: UAVObjectManager *objManager; - Waypoint *waypointObj; - PathAction *pathactionObj; flightDataModel *myModel; + uint completionCount; + uint completionSuccessCount; Waypoint *createWaypoint(int index, Waypoint *newWaypoint); void modelToWaypoint(int i, Waypoint::DataFields &data); @@ -56,6 +59,11 @@ private: PathAction *createPathAction(int index, PathAction *newAction); void modelToPathAction(int i, PathAction::DataFields &data); void pathActionToModel(int i, PathAction::DataFields &data); + +public slots: + void flightPlanSent(UAVObject *, bool success); + void flightPlanReceived(UAVObject *, bool success); + }; #endif // MODELUAVOPROXY_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 8f5f22d9d..c8b193a84 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -226,8 +226,10 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) table->setModel(model, selectionModel); waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel); UAVProxy = new ModelUavoProxy(this, model); - connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects())); - connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel())); + // sending and receiving is asynchronous + // TODO : buttons should be disabled while a send or receive is in progress + connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendFlightPlan())); + connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receiveFlightPlan())); #endif magicWayPoint = m_map->magicWPCreate(); magicWayPoint->setVisible(false); diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index 983863650..662a9fdd5 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -1,30 +1,24 @@ A waypoint command the pathplanner is to use at a certain waypoint - - - - - - - - + + + + + + + - + diff --git a/shared/uavobjectdefinition/waypoint.xml b/shared/uavobjectdefinition/waypoint.xml index 2a27f7fd5..912245163 100644 --- a/shared/uavobjectdefinition/waypoint.xml +++ b/shared/uavobjectdefinition/waypoint.xml @@ -1,12 +1,14 @@ A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module + - - + + + - + From f2115e270f36c2ca3556c69420fda269618f1c28 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 1 Dec 2013 19:42:41 +0100 Subject: [PATCH 036/113] OP-1122 OP-1133 UAVTalk.cpp - fixed stupid compilation error... --- ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index bd2a7d1c7..faf141717 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -37,7 +37,7 @@ #define UAVTALK_QXTLOG_DEBUG(args ...) #endif // UAVTALK_DEBUG -//#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC) +#define VERBOSE_FILTER(objId) //if (objId == 0xD23852DC) #define SYNC_VAL 0x3C From ab89d6a4910000dedb5c668919ae19ea2cf82c7e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 1 Dec 2013 21:44:14 +0100 Subject: [PATCH 037/113] OP-1122 OP-1125 fixed defect raised in OPReview-593 : two occurences of connection->respObj clearing were removed when they should not have been removed --- flight/uavtalk/uavtalk.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 95183d770..4f5d071b6 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -301,7 +301,9 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type if (respReceived == pdFALSE) { // Cancel transaction xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) + // non blocking call to make sure the value is reset to zero (binary sema) + xSemaphoreTake(connection->respSema, 0); + connection->respObjId = 0; xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->transLock); return -1; @@ -814,8 +816,9 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, */ static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId) { - if (connection->respType == type && connection->respObjId == objId && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { + if ((connection->respType == type) && (connection->respObjId == objId) && ((connection->respInstId == instId) || (connection->respInstId == UAVOBJ_ALL_INSTANCES))) { xSemaphoreGive(connection->respSema); + connection->respObjId = 0; } } From 4f9231e247ea5b6246da4adef79f38ae64924d2c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 4 Dec 2013 20:54:07 +0100 Subject: [PATCH 038/113] OP-1122 OP-1125 fixed regression in flight side uavtalk introduced in a recent commit --- flight/uavtalk/uavtalk.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 4f5d071b6..743f347c1 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -727,7 +727,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // All instances, not allowed for OBJ messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! - if (UAVObjUnpack(obj, instId, data)) { + if (UAVObjUnpack(obj, instId, data) == 0) { // Check if this object acks a pending OBJ_REQ message // any OBJ message can ack a pending OBJ_REQ message // even one that was not sent in response to the OBJ_REQ message @@ -853,6 +853,9 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) for (n = 0; n < numInst; ++n) { sendSingleObject(connection, type, objId, numInst - n - 1, obj); + if (UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) { + // TODO check semaphore and bail out if necessary + } } return 0; } else { From ff5927bc43e8f160a5ce08009110b9fe899c8f9c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 17:28:01 +0100 Subject: [PATCH 039/113] some changes to altitudehold, hope i get that done today... --- flight/modules/ManualControl/manualcontrol.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b1ab39276..8a7d0a7fb 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,7 +38,6 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" -#include "altholdsmoothed.h" #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" @@ -872,8 +871,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); + PositionStateData posState; + PositionStateGet(&posState); altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; @@ -883,23 +882,23 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesiredData.Velocity = 0; - altitudeHoldDesiredData.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; - altitudeHoldDesiredData.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; - altitudeHoldDesiredData.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { ; altitudeHoldDesiredData.Velocity = 0; - altitudeHoldDesiredData.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; } zeroed = true; } From a4d53c18aceb7d04bfce5a476430c904a32fc71e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 20:14:02 +0100 Subject: [PATCH 040/113] got rid of altholdsmoothed uavobject --- .../targets/boards/revolution/firmware/UAVObjects.inc | 1 - .../targets/boards/revoproto/firmware/UAVObjects.inc | 1 - .../targets/boards/simposix/firmware/UAVObjects.inc | 1 - .../src/plugins/uavobjects/uavobjects.pro | 2 -- shared/uavobjectdefinition/altholdsmoothed.xml | 11 ----------- 5 files changed, 16 deletions(-) delete mode 100644 shared/uavobjectdefinition/altholdsmoothed.xml diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 7a63dc9f2..8d7c97181 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -23,7 +23,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += gyrostate diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 80dab9e61..56eff01b1 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += gyrostate diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index be99363d3..0f8adf8f8 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += attitudesimulated diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d1b002994..0ca81220d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -32,7 +32,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ $$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ - $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ @@ -123,7 +122,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ - $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml deleted file mode 100644 index 1339e1337..000000000 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - The output on the kalman filter on altitude. - - - - - - - - From 623c25aa9918baf485eb00b39b1a0b87e272c1a0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 23:16:22 +0100 Subject: [PATCH 041/113] new design of altitude hold - warning not tested yet! --- flight/modules/AltitudeHold/altitudehold.c | 289 ++++++------------ flight/modules/ManualControl/manualcontrol.c | 4 +- .../altitudeholdsettings.xml | 13 +- 3 files changed, 99 insertions(+), 207 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 5bf40a086..180cb9e00 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -46,39 +46,37 @@ #include #include +#include #include -#include #include #include #include // object that will be updated by the module -#include -#include #include #include #include -#include #include #include #include // Private constants -#define MAX_QUEUE_SIZE 2 + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 4 -#define TIMEOUT_TRESHOLD 200000 #define DESIRED_UPDATE_RATE_MS 100 // milliseconds // Private types // Private variables -static xTaskHandle altitudeHoldTaskHandle; -static xQueueHandle queue; +static DelayedCallbackInfo *altitudeHoldCBInfo; static AltitudeHoldSettingsData altitudeHoldSettings; -static float throttleAlpha = 1.0f; -static float throttle_old = 0.0f; +static struct pid accelpid; +static float accelStateDown; + // Private functions -static void altitudeHoldTask(void *parameters); +static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); +static void AccelStateUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -87,8 +85,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); int32_t AltitudeHoldStart() { // Start main task - xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); + SettingsUpdatedCb(NULL); + DelayedCallbackDispatch(altitudeHoldCBInfo); return 0; } @@ -101,216 +99,111 @@ int32_t AltitudeHoldInitialize() { AltitudeHoldSettingsInitialize(); AltitudeHoldDesiredInitialize(); - AltHoldSmoothedInitialize(); // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + AccelStateConnectCallback(&AccelStateUpdatedCb); return 0; } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); -float tau; -float positionAlpha; -float velAlpha; -bool running = false; - -float velocity; -float velocityIntegral; -float altitudeIntegral; -float error; -float velError; -float derivative; -uint32_t timeval; -bool posUpdated; /** * Module thread, should not return. */ -static void altitudeHoldTask(__attribute__((unused)) void *parameters) +static void altitudeHoldTask(void) { - AltitudeHoldDesiredData altitudeHoldDesired; - StabilizationDesiredData stabilizationDesired; - AltHoldSmoothedData altHold; - VelocityStateData velocityData; - float dT; - float fblimit = 0; - portTickType thisTime, lastUpdateTime; - UAVObjEvent ev; + static float startThrottle =0.5f; - dT = 0; - timeval = 0; - lastUpdateTime = 0; - // Force update of the settings - SettingsUpdatedCb(&ev); - // Failsafe handling - uint32_t lastAltitudeHoldDesiredUpdate = 0; - bool enterFailSafe = false; - // Listen for updates. - AltitudeHoldDesiredConnectQueue(queue); - // PositionStateConnectQueue(queue); - FlightStatusConnectQueue(queue); - VelocityStateConnectQueue(queue); - bool altitudeHoldFlightMode = false; - running = false; - enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; + // make sure we run only when we are supposed to run + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + break; + default: + pid_zero(&accelpid); + StabilizationDesiredThrottleGet(&startThrottle); + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + break; + } - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - // initialize enable flag - altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; - // Main task loop - while (1) { - enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD; - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) { - if (!running) { - altitudeIntegral = 0; - } + // do the actual control loop(s) + AltitudeHoldDesiredData altitudeHoldDesired; + AltitudeHoldDesiredGet(&altitudeHoldDesired); + float positionStateDown; + PositionStateDownGet(&positionStateDown); + float velocityStateDown; + VelocityStateDownGet(&velocityStateDown); - // Todo: Add alarm if it should be running - continue; - } else if (ev.obj == FlightStatusHandle()) { - FlightStatusFlightModeGet(&flightMode); - altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; - if (altitudeHoldFlightMode && !running) { - AttitudeStateData attitudeState; - float q[4], Rbe[3][3]; - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - // Copy the current throttle as a starting point for integral - float initThrottle; - StabilizationDesiredThrottleGet(&initThrottle); - initThrottle *= Rbe[2][2]; // rotate into earth frame - if (initThrottle > 1) { - initThrottle = 1; - } else if (initThrottle < 0) { - initThrottle = 0; - } - error = 0; - altitudeHoldDesired.Velocity = 0; - altitudeHoldDesired.Altitude = altHold.Altitude; - altitudeIntegral = initThrottle; - velocityIntegral = 0; - running = true; - } else if (!altitudeHoldFlightMode) { - running = false; - lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); - } - } else if (ev.obj == VelocityStateHandle()) { - init = (init == WAITING_BARO) ? WAITIING_INIT : init; - dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; - timeval = PIOS_DELAY_GetRaw(); + // altitude control loop + float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; - AltHoldSmoothedGet(&altHold); - - VelocityStateGet(&velocityData); - - altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); - - float position; - PositionStateDownGet(&position); - altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude; - - AltHoldSmoothedSet(&altHold); - - // Verify that we are in altitude hold mode - uint8_t armed; - FlightStatusArmedGet(&armed); - if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) { - running = false; - } - - if (!running) { - lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); - continue; - } - - float lastError = error; - error = altitudeHoldDesired.Altitude - altHold.Altitude; - derivative = (error - lastError) / dT; - - velError = altitudeHoldDesired.Velocity - altHold.Velocity; - - // Compute altitude and velocity integral - altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT; - velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT; + // velocity control loop + float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f; - thisTime = xTaskGetTickCount(); - // Only update stabilizationDesired less frequently - if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { - continue; - } - lastUpdateTime = thisTime; + // compensate acceleration by rotation + // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q + // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation + // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical + // acceleration at the current tilt angle. + // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep + // integrals from winding in any direction + + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1,Rbe); + + float rotatedAccelDesired = realAccelDesired; + if (fabsf(Rbe[2][2])>1e-3f) { + rotatedAccelDesired /= Rbe[2][2]; + } else { + rotatedAccelDesired = accelStateDown; + } + + // acceleration control loop + float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS); - // Instead of explicit limit on integral you output limit feedback - StabilizationDesiredGet(&stabilizationDesired); - if (!enterFailSafe) { - stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral - + error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP] - + velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP] - + derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD]; + if (throttle>=1.0f) { + throttle=1.0f; + } + if (throttle<=0.0f) { + throttle=0.0f; + } + StabilizationDesiredData stab; + StabilizationDesiredGet(&stab); + stab.Roll = altitudeHoldDesired.Roll; + stab.Pitch = altitudeHoldDesired.Pitch; + stab.Yaw = altitudeHoldDesired.Yaw; + stab.Throttle = throttle; + stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + + StabilizationDesiredSet(&stab); - // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling - AttitudeStateData attitudeState; - float q[4], Rbe[3][3]; - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; - stabilizationDesired.Throttle /= throttlescale; - stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); - throttle_old = stabilizationDesired.Throttle; - fblimit = 0; + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); +} - if (stabilizationDesired.Throttle > 1) { - fblimit = stabilizationDesired.Throttle - 1; - stabilizationDesired.Throttle = 1; - } else if (stabilizationDesired.Throttle < 0) { - fblimit = stabilizationDesired.Throttle; - stabilizationDesired.Throttle = 0; - } - } else { - // shutdown motors - stabilizationDesired.Throttle = -1; - } - stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabilizationDesired.Roll = altitudeHoldDesired.Roll; - stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; - stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; - - StabilizationDesiredSet(&stabilizationDesired); - } else if (ev.obj == AltitudeHoldDesiredHandle()) { - // reset the failsafe timer - lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); - AltitudeHoldDesiredGet(&altitudeHoldDesired); - } - } +static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + float down; + AccelStatezGet(&down); + accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - positionAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.PositionTau); - velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); - - // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency) - if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) { - throttleAlpha = (float)DESIRED_UPDATE_RATE_MS / ((float)DESIRED_UPDATE_RATE_MS + 1000.0f / (2.0f * M_PI_F * altitudeHoldSettings.ThrottleFilterCutoff)); - } else { - throttleAlpha = 1.0f; - } + pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); + pid_zero(&accelpid); + accelStateDown=0.0f; } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 8a7d0a7fb..5913878ea 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -887,10 +887,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); altitudeHoldDesiredData.Altitude = posState.Down; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); altitudeHoldDesiredData.Altitude = posState.Down; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 806d86714..26491eb2f 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,13 +1,12 @@ Settings for the @ref AltitudeHold module - - - - - - - + + + + + + From 8832ea24f5b886f34ead71ed4099cd16cc230096 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 23:31:26 +0100 Subject: [PATCH 042/113] proper uncrustification --- flight/modules/AltitudeHold/altitudehold.c | 146 +++++++++++---------- 1 file changed, 74 insertions(+), 72 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 180cb9e00..84abe76f4 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -59,8 +59,8 @@ #include // Private constants -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define STACK_SIZE_BYTES 1024 #define DESIRED_UPDATE_RATE_MS 100 // milliseconds @@ -116,88 +116,90 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); */ static void altitudeHoldTask(void) { + static float startThrottle = 0.5f; - static float startThrottle =0.5f; + // make sure we run only when we are supposed to run + FlightStatusData flightStatus; - // make sure we run only when we are supposed to run - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: - break; - default: - pid_zero(&accelpid); - StabilizationDesiredThrottleGet(&startThrottle); - DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); - return; - break; - } + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + break; + default: + pid_zero(&accelpid); + StabilizationDesiredThrottleGet(&startThrottle); + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; - // do the actual control loop(s) - AltitudeHoldDesiredData altitudeHoldDesired; - AltitudeHoldDesiredGet(&altitudeHoldDesired); - float positionStateDown; - PositionStateDownGet(&positionStateDown); - float velocityStateDown; - VelocityStateDownGet(&velocityStateDown); + break; + } - // altitude control loop - float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + // do the actual control loop(s) + AltitudeHoldDesiredData altitudeHoldDesired; + AltitudeHoldDesiredGet(&altitudeHoldDesired); + float positionStateDown; + PositionStateDownGet(&positionStateDown); + float velocityStateDown; + VelocityStateDownGet(&velocityStateDown); - // velocity control loop - float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f; + // altitude control loop + float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + + // velocity control loop + float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f; - // compensate acceleration by rotation - // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q - // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation - // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical - // acceleration at the current tilt angle. - // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep - // integrals from winding in any direction - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rbe[3][3]; - Quaternion2R(&attitudeState.q1,Rbe); - - float rotatedAccelDesired = realAccelDesired; - if (fabsf(Rbe[2][2])>1e-3f) { - rotatedAccelDesired /= Rbe[2][2]; - } else { - rotatedAccelDesired = accelStateDown; - } - - // acceleration control loop - float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS); + // compensate acceleration by rotation + // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q + // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation + // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical + // acceleration at the current tilt angle. + // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep + // integrals from winding in any direction - if (throttle>=1.0f) { - throttle=1.0f; - } - if (throttle<=0.0f) { - throttle=0.0f; - } - StabilizationDesiredData stab; - StabilizationDesiredGet(&stab); - stab.Roll = altitudeHoldDesired.Roll; - stab.Pitch = altitudeHoldDesired.Pitch; - stab.Yaw = altitudeHoldDesired.Yaw; - stab.Throttle = throttle; - stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - - StabilizationDesiredSet(&stab); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1, Rbe); - DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + float rotatedAccelDesired = realAccelDesired; + if (fabsf(Rbe[2][2]) > 1e-3f) { + rotatedAccelDesired /= Rbe[2][2]; + } else { + rotatedAccelDesired = accelStateDown; + } + + // acceleration control loop + float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); + + if (throttle >= 1.0f) { + throttle = 1.0f; + } + if (throttle <= 0.0f) { + throttle = 0.0f; + } + StabilizationDesiredData stab; + StabilizationDesiredGet(&stab); + stab.Roll = altitudeHoldDesired.Roll; + stab.Pitch = altitudeHoldDesired.Pitch; + stab.Yaw = altitudeHoldDesired.Yaw; + stab.Throttle = throttle; + stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + + StabilizationDesiredSet(&stab); + + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); } static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - float down; - AccelStatezGet(&down); - accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); + float down; + + AccelStatezGet(&down); + accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) @@ -205,5 +207,5 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); pid_zero(&accelpid); - accelStateDown=0.0f; + accelStateDown = 0.0f; } From 981fc4321d65bcf744bdb1d96d08c4d7d2c2734f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 10:32:24 +0100 Subject: [PATCH 043/113] removed requirement for non existing task --- flight/libraries/sanitycheck.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 51442f48e..ab53e2302 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -113,8 +113,6 @@ int32_t configuration_check() case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: From 90f689cfa13329615b249d2d25907510e44c8940 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 10:44:17 +0100 Subject: [PATCH 044/113] throttle does not accelerate downwards but upwards - switched sign --- flight/modules/AltitudeHold/altitudehold.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 84abe76f4..ffe5b5cc3 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -171,7 +171,7 @@ static void altitudeHoldTask(void) } // acceleration control loop - float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); + float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); if (throttle >= 1.0f) { throttle = 1.0f; From 4ad0e730dad417f03f52f537d98171f2cc56f74e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 12:23:16 +0100 Subject: [PATCH 045/113] added status uavobject for easier debugging --- flight/modules/AltitudeHold/altitudehold.c | 14 +++++++++++--- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 ++ shared/uavobjectdefinition/altitudeholddesired.xml | 2 +- 6 files changed, 17 insertions(+), 4 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index ffe5b5cc3..78ec27332 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -51,6 +51,7 @@ #include #include #include // object that will be updated by the module +#include #include #include #include @@ -99,6 +100,7 @@ int32_t AltitudeHoldInitialize() { AltitudeHoldSettingsInitialize(); AltitudeHoldDesiredInitialize(); + AltitudeHoldStatusInitialize(); // Create object queue @@ -135,6 +137,9 @@ static void altitudeHoldTask(void) break; } + AltitudeHoldStatusData altitudeHoldStatus; + AltitudeHoldStatusGet(&altitudeHoldStatus); + // do the actual control loop(s) AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredGet(&altitudeHoldDesired); @@ -144,11 +149,12 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; // velocity control loop - float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f; + altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + AltitudeHoldStatusSet(&altitudeHoldStatus); // compensate acceleration by rotation // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q @@ -163,12 +169,14 @@ static void altitudeHoldTask(void) float Rbe[3][3]; Quaternion2R(&attitudeState.q1, Rbe); - float rotatedAccelDesired = realAccelDesired; + float rotatedAccelDesired = altitudeHoldStatus.AccelerationDesired; +#if 0 if (fabsf(Rbe[2][2]) > 1e-3f) { rotatedAccelDesired /= Rbe[2][2]; } else { rotatedAccelDesired = accelStateDown; } +#endif // acceleration control loop float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 8d7c97181..33a47e757 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -98,6 +98,7 @@ UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += poilocation diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 56eff01b1..ee6c02bbd 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -96,6 +96,7 @@ UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += poilocation diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 0f8adf8f8..e9fc43178 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -101,6 +101,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 0ca81220d..8fb4d384e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -34,6 +34,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ + $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \ @@ -124,6 +125,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 116e82306..7a69227fd 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -8,7 +8,7 @@ - + From 3cea14a809ced88e7164b03098974c9166e40e85 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 13:06:28 +0100 Subject: [PATCH 046/113] added missing object, added more debug output --- flight/modules/AltitudeHold/altitudehold.c | 2 ++ shared/uavobjectdefinition/altitudeholdstatus.xml | 12 ++++++++++++ 2 files changed, 14 insertions(+) create mode 100644 shared/uavobjectdefinition/altitudeholdstatus.xml diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 78ec27332..771aca1e6 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -154,6 +154,8 @@ static void altitudeHoldTask(void) // velocity control loop altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + altitudeHoldStatus.AccelerationFiltered = accelStateDown; + AltitudeHoldStatusSet(&altitudeHoldStatus); // compensate acceleration by rotation diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml new file mode 100644 index 000000000..928496f51 --- /dev/null +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -0,0 +1,12 @@ + + + Status Data from Altitude Hold Control Loops + + + + + + + + + From 5cc8dedadf09ae68cf037fdd4f3b372d0b13ac37 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 13:07:26 +0100 Subject: [PATCH 047/113] uncrustify changed things --- flight/modules/AltitudeHold/altitudehold.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 771aca1e6..c92014b97 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,10 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; // velocity control loop - altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; altitudeHoldStatus.AccelerationFiltered = accelStateDown; From 66b0ffd682371f7262a3514cd709e036dfac2900 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 13:35:00 +0100 Subject: [PATCH 048/113] small fix to baro state filter - more init cycles --- flight/modules/StateEstimation/filterbaro.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 739fa5d72..18078efd1 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -37,6 +37,7 @@ // Private constants #define STACK_REQUIRED 64 +#define INIT_CYCLES 1000 // Private types struct data { @@ -67,7 +68,7 @@ static int32_t init(stateFilter *self) struct data *this = (struct data *)self->localdata; this->baroOffset = 0.0f; - this->first_run = 100; + this->first_run = INIT_CYCLES; RevoSettingsInitialize(); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); @@ -82,7 +83,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0]; + this->baroOffset = (INIT_CYCLES - this->first_run) / INIT_CYCLES * this->baroOffset + (this->first_run / INIT_CYCLES) * state->baro[0]; this->baroAlt = this->baroOffset; this->first_run--; UNSET_MASK(state->updated, SENSORUPDATES_baro); From 84af4b7651683cc972e51bb30e1a82751577f11d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 14:00:09 +0100 Subject: [PATCH 049/113] fixes to barofilter --- flight/modules/StateEstimation/filterbaro.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 18078efd1..a44927392 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -37,7 +37,7 @@ // Private constants #define STACK_REQUIRED 64 -#define INIT_CYCLES 1000 +#define INIT_CYCLES 100 // Private types struct data { @@ -83,8 +83,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->baroOffset = (INIT_CYCLES - this->first_run) / INIT_CYCLES * this->baroOffset + (this->first_run / INIT_CYCLES) * state->baro[0]; - this->baroAlt = this->baroOffset; + this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0]; + this->baroAlt = 0; this->first_run--; UNSET_MASK(state->updated, SENSORUPDATES_baro); } From 95d52c6b08b3f580e9ca8e2f83c4b2fa09504fd7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 14:00:18 +0100 Subject: [PATCH 050/113] fixed signs in control loop --- flight/modules/AltitudeHold/altitudehold.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index c92014b97..153ffcf24 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,10 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (altitudeHoldDesired.Altitude - positionStateDown) + altitudeHoldDesired.Velocity; // velocity control loop - altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (altitudeHoldStatus.VelocityDesired - velocityStateDown) - 9.81f; altitudeHoldStatus.AccelerationFiltered = accelStateDown; From 612e66367af55daafaff3581e1ec18b145b0e51e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 8 Dec 2013 14:30:51 +0100 Subject: [PATCH 051/113] OP-1122 OP-1125 made GCS telemetry and uavtalk a bit more robust (more error case handling) minor cleanup and code reorganization some preparatory work to read from IODevice from its own thread --- .../src/plugins/uavtalk/telemetry.cpp | 66 +++++--- .../src/plugins/uavtalk/telemetrymanager.cpp | 44 ++++- .../src/plugins/uavtalk/telemetrymanager.h | 14 ++ .../src/plugins/uavtalk/uavtalk.cpp | 157 ++++++++++-------- .../src/plugins/uavtalk/uavtalk.h | 19 ++- 5 files changed, 196 insertions(+), 104 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 7ddca5f47..499384491 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -230,11 +230,6 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success) // Lookup the transaction in the transaction map. ObjectTransactionInfo *transInfo = findTransaction(obj); if (transInfo) { - // Remove this transaction as it's complete. - transInfo->timer->stop(); - closeTransaction(transInfo); - delete transInfo; - // Send signal if (success) { #ifdef VERBOSE_TELEMETRY qDebug() << "Telemetry - transaction successful for object" << obj->toStringBrief(); @@ -242,7 +237,13 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success) } else { qWarning() << "Telemetry - !!! transaction failed for object" << obj->toStringBrief(); } + + // Remove this transaction as it's complete. + closeTransaction(transInfo); + + // Send signal obj->emitTransactionCompleted(success); + // Process new object updates from queue processObjectQueue(); } else { @@ -255,30 +256,33 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success) */ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) { - transInfo->timer->stop(); // Check if more retries are pending if (transInfo->retriesRemaining > 0) { #ifdef VERBOSE_TELEMETRY qDebug().nospace() << "Telemetry - transaction timed out for object " << transInfo->obj->toStringBrief() << ", retrying..."; #endif - --transInfo->retriesRemaining; - processObjectTransaction(transInfo); ++txRetries; + --transInfo->retriesRemaining; + + // Retry the transaction + processObjectTransaction(transInfo); } else { - // Stop the timer. - transInfo->timer->stop(); + qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief(); + + ++txErrors; + // Terminate transaction utalk->cancelTransaction(transInfo->obj); - // Send signal - qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief(); - transInfo->obj->emitTransactionCompleted(false); + // Remove this transaction as it's complete. - // FIXME : also remove transaction from UAVTalk transaction map + UAVObject *obj = transInfo->obj; closeTransaction(transInfo); - delete transInfo; + + // Send signal + obj->emitTransactionCompleted(false); + // Process new object updates from queue processObjectQueue(); - ++txErrors; } } @@ -288,24 +292,33 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) { // Initiate transaction + bool sent = false; if (transInfo->objRequest) { #ifdef VERBOSE_TELEMETRY - qDebug().nospace() << "Telemetry - sending object request for " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); + qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); #endif - utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); + sent = utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); } else { #ifdef VERBOSE_TELEMETRY qDebug().nospace() << "Telemetry - sending object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); #endif - utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); + sent = utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); } - // Start timer if a response is expected + // Check if a response is needed now or will arrive asynchronously if (transInfo->objRequest || transInfo->acked) { - transInfo->timer->start(REQ_TIMEOUT_MS); - } else { - // Otherwise, remove this transaction as it's complete. + if (sent) { + // Start timer if a response is expected + transInfo->timer->start(REQ_TIMEOUT_MS); + } + else { + // message was not sent, the transaction will not complete and will timeout + // there is no need to wait to close the transaction and notify of completion failure + //transactionCompleted(transInfo->obj, false); + } + } + else { + // not transacted, so just close the transaction with no notification of completion closeTransaction(transInfo); - delete transInfo; } } @@ -380,7 +393,7 @@ void Telemetry::processObjectQueue() // TODO make the above logic a reality... if (findTransaction(objInfo.obj)) { qWarning() << "Telemetry - !!! Making request for a object " << objInfo.obj->toStringBrief() << " for which a request is already in progress"; - objInfo.obj->emitTransactionCompleted(false); + //objInfo.obj->emitTransactionCompleted(false); return; } UAVObject::Metadata metadata = objInfo.obj->getMetadata(); @@ -599,6 +612,7 @@ void Telemetry::closeTransaction(ObjectTransactionInfo *trans) // Keep the map even if it is empty // There are at most 100 different object IDs... } + delete trans; } void Telemetry::closeAllTransactions() @@ -626,7 +640,7 @@ ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent) telem = 0; // Setup transaction timer timer = new QTimer(this); - timer->stop(); + timer->setSingleShot(true); connect(timer, SIGNAL(timeout()), this, SLOT(timeout())); } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp index 008e3af73..c2145bc7b 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp @@ -30,8 +30,7 @@ #include #include -TelemetryManager::TelemetryManager() : - autopilotConnected(false) +TelemetryManager::TelemetryManager() : autopilotConnected(false) { moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); // Get UAVObjectManager instance @@ -44,7 +43,8 @@ TelemetryManager::TelemetryManager() : } TelemetryManager::~TelemetryManager() -{} +{ +} bool TelemetryManager::isConnected() { @@ -62,6 +62,29 @@ void TelemetryManager::onStart() utalk = new UAVTalk(device, objMngr); telemetry = new Telemetry(utalk, objMngr); telemetryMon = new TelemetryMonitor(objMngr, telemetry); + + if (false) { + // UAVTalk must be thread safe and for that: + // 1- all public methods must lock a mutex + // 2- the reader thread must lock that mutex too + // The reader thread locks the mutex once a packet is read and decoded. + // It is assumed that the UAVObjectManager is thread safe + + // Create the reader and move it to the reader thread + IODeviceReader *reader = new IODeviceReader(utalk); + reader->moveToThread(&readerThread); + // The reader will be deleted (later) when the thread finishes + connect(&readerThread, &QThread::finished, reader, &QObject::deleteLater); + // Connect IO device to reader + connect(device, SIGNAL(readyRead()), reader, SLOT(read())); + // start the reader thread + readerThread.start(); + } + else { + // Connect IO device to reader + connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream())); + } + connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect())); connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect())); } @@ -70,6 +93,11 @@ void TelemetryManager::onStart() void TelemetryManager::stop() { emit myStop(); + + if (false) { + readerThread.quit(); + readerThread.wait(); + } } void TelemetryManager::onStop() @@ -92,3 +120,13 @@ void TelemetryManager::onDisconnect() autopilotConnected = false; emit disconnected(); } + +IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk) +{ +} + +void IODeviceReader::read() +{ + uavTalk->processInputStream(); +} + diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h index c4e7301ec..9f575371d 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h @@ -66,6 +66,20 @@ private: TelemetryMonitor *telemetryMon; QIODevice *device; bool autopilotConnected; + QThread readerThread; +}; + + +class IODeviceReader : public QObject +{ + Q_OBJECT +public: + IODeviceReader(UAVTalk *uavTalk); + + UAVTalk *uavTalk; + +public slots: + void read(); }; #endif // TELEMETRYMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index faf141717..d4db0132c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -25,11 +25,13 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavtalk.h" -#include -#include #include #include +#include +#include +#include + // #define UAVTALK_DEBUG #ifdef UAVTALK_DEBUG #define UAVTALK_QXTLOG_DEBUG(args ...) @@ -64,20 +66,13 @@ const quint8 UAVTalk::crc_table[256] = { /** * Constructor */ -UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) +UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) : io(iodev), objMngr(objMngr), mutex(QMutex::Recursive) { - io = iodev; - - this->objMngr = objMngr; - rxState = STATE_SYNC; rxPacketLength = 0; - mutex = new QMutex(QMutex::Recursive); - memset(&stats, 0, sizeof(ComStats)); - connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::Internal::GeneralSettings *settings = pm->getObject(); useUDPMirror = settings->useUDPMirror(); @@ -94,9 +89,9 @@ UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) UAVTalk::~UAVTalk() { - // According to Qt, it is not necessary to disconnect upon - // object deletion. - // disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); + // According to Qt, it is not necessary to disconnect upon object deletion. + // disconnect(io, SIGNAL(readyRead()), worker, SLOT(processInputStream())); + closeAllTransactions(); } @@ -106,7 +101,7 @@ UAVTalk::~UAVTalk() */ void UAVTalk::resetStats() { - QMutexLocker locker(mutex); + QMutexLocker locker(&mutex); memset(&stats, 0, sizeof(ComStats)); } @@ -116,26 +111,11 @@ void UAVTalk::resetStats() */ UAVTalk::ComStats UAVTalk::getStats() { - QMutexLocker locker(mutex); + QMutexLocker locker(&mutex); return stats; } -/** - * Called each time there are data in the input buffer - */ -void UAVTalk::processInputStream() -{ - quint8 tmp; - - if (io && io->isReadable()) { - while (io->bytesAvailable() > 0) { - io->read((char *)&tmp, 1); - processInputByte(tmp); - } - } -} - void UAVTalk::dummyUDPRead() { QUdpSocket *socket = qobject_cast(sender()); @@ -147,6 +127,35 @@ void UAVTalk::dummyUDPRead() } } +/** + * Send the specified object through the telemetry link. + * \param[in] obj Object to send + * \param[in] acked Selects if an ack is required + * \param[in] allInstances If set true then all instances will be updated + * \return Success (true), Failure (false) + */ +bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) +{ + QMutexLocker locker(&mutex); + + quint16 instId = 0; + + if (allInstances) { + instId = ALL_INSTANCES; + } + else if (obj) { + instId = obj->getInstID(); + } + bool success = false; + if (acked) { + success = objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj); + } else { + success = objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj); + } + + return success; +} + /** * Request an update for the specified object, on success the object data would have been * updated by the GCS. @@ -156,7 +165,7 @@ void UAVTalk::dummyUDPRead() */ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) { - QMutexLocker locker(mutex); + QMutexLocker locker(&mutex); quint16 instId = 0; @@ -169,38 +178,12 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj); } -/** - * Send the specified object through the telemetry link. - * \param[in] obj Object to send - * \param[in] acked Selects if an ack is required - * \param[in] allInstances If set true then all instances will be updated - * \return Success (true), Failure (false) - */ -bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) -{ - QMutexLocker locker(mutex); - - quint16 instId = 0; - - if (allInstances) { - instId = ALL_INSTANCES; - } - else if (obj) { - instId = obj->getInstID(); - } - if (acked) { - return objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj); - } else { - return objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj); - } -} - /** * Cancel a pending transaction */ void UAVTalk::cancelTransaction(UAVObject *obj) { - QMutexLocker locker(mutex); + QMutexLocker locker(&mutex); if (io.isNull()) { return; @@ -240,6 +223,21 @@ bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVO } } +/** + * Called each time there are data in the input buffer + */ +void UAVTalk::processInputStream() +{ + quint8 tmp; + + if (io && io->isReadable()) { + while (io->bytesAvailable() > 0) { + io->read((char *)&tmp, 1); + processInputByte(tmp); + } + } +} + /** * Process an byte from the telemetry stream. * \param[in] rxbyte Received byte @@ -446,18 +444,18 @@ bool UAVTalk::processInputByte(quint8 rxbyte) break; } - mutex->lock(); + mutex.lock(); receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); - if (useUDPMirror) { - udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); - } - stats.rxObjectBytes += rxLength; stats.rxObjects++; - mutex->unlock(); + mutex.unlock(); + + if (useUDPMirror) { + udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); + } rxState = STATE_SYNC; UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)"); @@ -661,7 +659,17 @@ void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *o } Transaction *trans = findTransaction(objId, instId); if (trans && trans->respType == type) { - if (trans->respInstId != ALL_INSTANCES || instId == 0) { + if (trans->respInstId == ALL_INSTANCES) { + if (instId == 0) { + // last instance received, complete transaction + closeTransaction(trans); + emit transactionCompleted(obj, true); + } + else { + // TODO extend timeout? + } + } + else { closeTransaction(trans); emit transactionCompleted(obj, true); } @@ -705,14 +713,27 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje // Process message type if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { if (allInstances) { - // Get number of instances - quint32 numInst = objMngr->getNumInstances(objId); // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) + quint32 numInst = objMngr->getNumInstances(objId); for (quint32 n = 0; n < numInst; ++n) { quint32 i = numInst - n - 1; + UAVObject *o = objMngr->getObject(objId, i); - transmitSingleObject(type, objId, i, o); + if (!transmitSingleObject(type, objId, i, o)) { + return false; + } + + if (false) { + // yield + mutex.unlock(); + // going back to the event loop is necessary to allow timeout events to be fired + // but don't allow user events as the event can cause the main thread to reenter UAVTalk + // the timer event suffers from the same issue but this case is handled + //QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents, 10); + QThread::msleep(1); + mutex.lock(); + } } return true; } else { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index e5be192f7..15f86cdaa 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -27,19 +27,22 @@ #ifndef UAVTALK_H #define UAVTALK_H +#include "uavobjectmanager.h" +#include "uavtalk_global.h" + #include #include #include #include #include -#include -#include "uavobjectmanager.h" -#include "uavtalk_global.h" +#include #include class UAVTALK_EXPORT UAVTalk : public QObject { Q_OBJECT + friend class IODeviceReader; + public: static const quint16 ALL_INSTANCES = 0xFFFF; @@ -56,17 +59,19 @@ public: UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr); ~UAVTalk(); + + ComStats getStats(); + void resetStats(); + bool sendObject(UAVObject *obj, bool acked, bool allInstances); bool sendObjectRequest(UAVObject *obj, bool allInstances); void cancelTransaction(UAVObject *obj); - ComStats getStats(); - void resetStats(); signals: void transactionCompleted(UAVObject *obj, bool success); private slots: - void processInputStream(void); + void processInputStream(); void dummyUDPRead(); private: @@ -106,7 +111,7 @@ private: // Variables QPointer io; UAVObjectManager *objMngr; - QMutex *mutex; + QMutex mutex; QMap *> transMap; quint8 rxBuffer[MAX_PACKET_LENGTH]; quint8 txBuffer[MAX_PACKET_LENGTH]; From 95b5f38556c91f4474708b66e039f29b1a489ab3 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 8 Dec 2013 14:34:11 +0100 Subject: [PATCH 052/113] OP-1122 OP-1125 flight side UAVObjectManager was wrongly emitting EV_UPDATED_MANUAL events when new uavobject instances were created. Now properly emits EV_UPDATED. --- flight/uavobjects/uavobjectmanager.c | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 0ed0e1cfe..9b2209d8f 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -183,6 +183,7 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask); static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb); +static void autoUpdated(UAVObjHandle obj_handle, uint16_t instId); #if defined(PIOS_USE_SETTINGS_ON_SDCARD) && defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) #error Both PIOS_USE_SETTINGS_ON_SDCARD and PIOS_INCLUDE_FLASH_LOGFS_SETTINGS. Only one settings storage allowed. @@ -399,8 +400,8 @@ UAVObjHandle UAVObjRegister(uint32_t id, } // fire events for outer object and its embedded meta object - UAVObjInstanceUpdated((UAVObjHandle)uavo_data, 0); - UAVObjInstanceUpdated((UAVObjHandle) & (uavo_data->metaObj), 0); + autoUpdated((UAVObjHandle)uavo_data, 0); + autoUpdated((UAVObjHandle) & (uavo_data->metaObj), 0); unlock_exit: xSemaphoreGiveRecursive(mutex); @@ -1819,6 +1820,19 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId) xSemaphoreGiveRecursive(mutex); } +/** + * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). + * \param[in] obj The object handle + * \param[in] instId The object instance ID + */ +static void autoUpdated(UAVObjHandle obj_handle, uint16_t instId) +{ + PIOS_Assert(obj_handle); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); + xSemaphoreGiveRecursive(mutex); +} + /** * Iterate through all objects in the list. * \param iterator This function will be called once for each object, @@ -1925,7 +1939,7 @@ static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId) ((struct UAVOMulti *)obj)->num_instances++; // Fire event - UAVObjInstanceUpdated((UAVObjHandle)obj, instId); + autoUpdated((UAVObjHandle)obj, instId); // Done return InstanceDataOffset(instEntry); From 75842cb64840396b42acf3b88ba0734888f83899 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:21:05 +0100 Subject: [PATCH 053/113] =?UTF-8?q?OP-1139:=20perform=20second=20order=20l?= =?UTF-8?q?ow=20temperature=20compensation.=20It=20follows=20the=20procedu?= =?UTF-8?q?re=20described=20in=20MS5611=20datasheet(http://www.meas-spec.c?= =?UTF-8?q?om/downloads/MS5611-01BA03.pdf,=20page=208)=20to=20perform=20lo?= =?UTF-8?q?w(20=C2=B0)=20and=20very=20low(-15=C2=B0C)=20temperature=20comp?= =?UTF-8?q?ensation.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- flight/pios/common/pios_ms5611.c | 32 ++++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index 1d1a289e7..aeb99de45 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -51,7 +51,7 @@ static int64_t Temperature; static int32_t lastConversionStart; static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); - +static int64_t t2; // Move into proper driver structure with cfg stored static uint32_t oversampling; static const struct pios_ms5611_cfg *dev_cfg; @@ -74,6 +74,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) uint8_t data[2]; + // reset calibration values + t2 = 0; + /* Calibration parameters */ for (int i = 0; i < 6; i++) { PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); @@ -190,17 +193,34 @@ int32_t PIOS_MS5611_ReadADC(void) } else { int64_t Offset; int64_t Sens; + // used for second order temperature compensation + int64_t Offset2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { return -1; } + // check if temperature is less than 20°C + if(Temperature < 2000){ + Offset2 = 5 * (Temperature - 2000) >> 1; + Sens2 = Offset2 >> 1; + t2 = (deltaTemp*deltaTemp) >> 31; + // Apply the "Very low temperature compensation" when temp is less than -15°C + if(Temperature < -1500){ + int64_t tcorr = (Temperature + 1500)*(Temperature + 1500); + Offset2 += 7 * tcorr; + Sens2 += (11 * tcorr) >> 1; + } + } else { + t2 = 0; + Offset2 = 0; + Sens2 = 0; + } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); - - Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7); + Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; Sens = ((int64_t)CalibData.C[0]) << 15; - Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8); - + Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2; Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; } return 0; @@ -211,7 +231,7 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float)Temperature) / 100.0f; + return ((float)(Temperature - t2)) / 100.0f; } /** From 13b45b2b7846055ed55266c0bdec21690ff76e81 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:44:13 +0100 Subject: [PATCH 054/113] OP.1139: uncrustify --- flight/pios/common/pios_ms5611.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index aeb99de45..f21fff0fc 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -195,27 +195,27 @@ int32_t PIOS_MS5611_ReadADC(void) int64_t Sens; // used for second order temperature compensation int64_t Offset2 = 0; - int64_t Sens2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { return -1; } // check if temperature is less than 20°C - if(Temperature < 2000){ + if (Temperature < 2000) { Offset2 = 5 * (Temperature - 2000) >> 1; - Sens2 = Offset2 >> 1; - t2 = (deltaTemp*deltaTemp) >> 31; + Sens2 = Offset2 >> 1; + t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C - if(Temperature < -1500){ - int64_t tcorr = (Temperature + 1500)*(Temperature + 1500); + if (Temperature < -1500) { + int64_t tcorr = (Temperature + 1500) * (Temperature + 1500); Offset2 += 7 * tcorr; - Sens2 += (11 * tcorr) >> 1; + Sens2 += (11 * tcorr) >> 1; } } else { - t2 = 0; + t2 = 0; Offset2 = 0; - Sens2 = 0; + Sens2 = 0; } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; From 634ba79dcb4ee858524d34c64d386a9eb48818cf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:46:28 +0100 Subject: [PATCH 055/113] OP-1141: allow to input a barometer bias model. It uses a 3rd degree polynomial to model pressure bias as a function of temperature. --- flight/modules/Altitude/revolution/altitude.c | 21 ++++++++++++++----- shared/uavobjectdefinition/revosettings.xml | 4 +++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index c66f52ae8..27a26278d 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -40,22 +40,24 @@ #include "altitude.h" #include "barosensor.h" // object that will be updated by the module +#include "revosettings.h" #if defined(PIOS_INCLUDE_HCSR04) #include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants -#define STACK_SIZE_BYTES 500 +#define STACK_SIZE_BYTES 550 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types // Private variables static xTaskHandle taskHandle; - +static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; // Private functions static void altitudeTask(void *parameters); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -77,6 +79,9 @@ int32_t AltitudeStart() int32_t AltitudeInitialize() { BaroSensorInitialize(); + RevoSettingsInitialize(); + RevoSettingsConnectCallback(&SettingsUpdatedCb); + #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); #endif @@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters) // Undef for normal operation // #define PIOS_MS5611_SLOW_TEMP_RATE 20 + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); + #ifdef PIOS_MS5611_SLOW_TEMP_RATE uint8_t temp_press_interleave_count = 1; #endif @@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters) vTaskDelay(PIOS_MS5611_GetDelay()); PIOS_MS5611_ReadADC(); - temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); + float temp2 = temp * temp; + float correction = baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - - float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press - correction) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; @@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) } } +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); +} /** * @} * @} diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 0cf9a4561..8d634d1cd 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -7,7 +7,9 @@ Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062 Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. --> - + + From 44269b67623069b576294eb8c545227bd56f7e09 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 02:02:30 +0100 Subject: [PATCH 056/113] OP-1139: export corrected pressure value to uavobject --- flight/modules/Altitude/revolution/altitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 27a26278d..d46c7c5bb 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -164,9 +164,9 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); float temp2 = temp * temp; - float correction = baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; + press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - float altitude = 44330.0f * (1.0f - powf((press - correction) / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; From e0115a25b0ab54068fecc764605c5235e1273473 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 11 Dec 2013 22:22:28 +0100 Subject: [PATCH 057/113] OP-1122 OP-1125 cleaned up GCS side uavtalk includes --- .../cfg_vehicletypes/configcustomwidget.h | 1 - .../cfg_vehicletypes/configfixedwingwidget.h | 2 +- .../configgroundvehiclewidget.h | 2 +- .../cfg_vehicletypes/configmultirotorwidget.h | 1 - .../src/plugins/config/configgadgetwidget.cpp | 2 ++ .../src/plugins/config/configgadgetwidget.h | 8 ++++---- .../src/plugins/config/configinputwidget.cpp | 6 ++---- .../src/plugins/config/configoutputwidget.cpp | 17 ++++++++--------- .../src/plugins/config/configplugin.h | 1 - .../openpilotgcs/src/plugins/hitl/isimulator.h | 11 +++++------ .../openpilotgcs/src/plugins/hitl/simulator.cpp | 8 +++++--- .../openpilotgcs/src/plugins/hitl/simulator.h | 14 +++++++------- .../src/plugins/notify/notifyplugin.cpp | 12 +++++++----- .../src/plugins/notify/notifyplugin.h | 2 -- .../src/plugins/scope/scopegadgetwidget.cpp | 1 - .../plugins/setupwizard/pages/autoupdatepage.h | 1 - .../systemhealth/systemhealthgadgetwidget.cpp | 4 +++- .../systemhealth/systemhealthgadgetwidget.h | 2 +- .../plugins/telemetry/monitorgadgetfactory.cpp | 5 +++-- .../uavobjectwidgetutils/configtaskwidget.cpp | 10 +++++++--- .../uavobjectwidgetutils/smartsavebutton.h | 1 - .../src/plugins/uploader/runningdevicewidget.h | 10 +++++----- .../plugins/uploader/uploadergadgetwidget.cpp | 7 +++++-- .../src/plugins/uploader/uploadergadgetwidget.h | 1 - 24 files changed, 66 insertions(+), 63 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h index faa89cc9c..fbef8939e 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h @@ -33,7 +33,6 @@ #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" -#include "uavtalk/telemetrymanager.h" #include #include diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 65c03c86e..f0b786379 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -33,7 +33,7 @@ #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" -#include "uavtalk/telemetrymanager.h" + #include #include #include diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h index 94ef93e0c..72d7d0fff 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h @@ -33,7 +33,7 @@ #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" -#include "uavtalk/telemetrymanager.h" + #include #include #include diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h index 181c06875..dde2ced06 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h @@ -33,7 +33,6 @@ #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" -#include "uavtalk/telemetrymanager.h" #include #include diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index b71333c4f..6e932e870 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -42,6 +42,8 @@ #include "defaulthwsettingswidget.h" #include "uavobjectutilmanager.h" +#include + #include #include #include diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index a2d04e793..45eadd8b5 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -27,18 +27,18 @@ #ifndef CONFIGGADGETWIDGET_H #define CONFIGGADGETWIDGET_H -#include "uavtalk/telemetrymanager.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" #include "objectpersistence.h" +#include "utils/pathutils.h" +#include "utils/mytabbedstackwidget.h" +#include "../uavobjectwidgetutils/configtaskwidget.h" + #include #include #include -#include "utils/pathutils.h" #include -#include "utils/mytabbedstackwidget.h" -#include "../uavobjectwidgetutils/configtaskwidget.h" class ConfigGadgetWidget : public QWidget { Q_OBJECT diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index d1040724f..c2d119ac8 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -27,7 +27,8 @@ #include "configinputwidget.h" -#include "uavtalk/telemetrymanager.h" +#include +#include #include #include @@ -41,9 +42,6 @@ #include #include -#include -#include - #define ACCESS_MIN_MOVE -3 #define ACCESS_MAX_MOVE 3 #define STICK_MIN_MOVE -8 diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index ee15cf188..9ca2ed943 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -29,7 +29,14 @@ #include "outputchannelform.h" #include "configvehicletypewidget.h" -#include "uavtalk/telemetrymanager.h" +#include "mixersettings.h" +#include "actuatorcommand.h" +#include "actuatorsettings.h" +#include "systemalarms.h" +#include "systemsettings.h" +#include "uavsettingsimportexport/uavsettingsimportexportfactory.h" +#include +#include #include #include @@ -40,14 +47,6 @@ #include #include #include -#include "mixersettings.h" -#include "actuatorcommand.h" -#include "actuatorsettings.h" -#include "systemalarms.h" -#include "systemsettings.h" -#include "uavsettingsimportexport/uavsettingsimportexportfactory.h" -#include -#include ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent), wasItMe(false) { diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.h b/ground/openpilotgcs/src/plugins/config/configplugin.h index c47cfd791..2f8ead7e2 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.h +++ b/ground/openpilotgcs/src/plugins/config/configplugin.h @@ -31,7 +31,6 @@ #include #include #include -#include "uavtalk/telemetrymanager.h" #include "objectpersistence.h" #include diff --git a/ground/openpilotgcs/src/plugins/hitl/isimulator.h b/ground/openpilotgcs/src/plugins/hitl/isimulator.h index 12124a5bf..f9b064d9b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/isimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/isimulator.h @@ -1,12 +1,6 @@ #ifndef ISIMULATOR_H #define ISIMULATOR_H - -#include -#include -#include -#include -#include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" #include "actuatordesired.h" #include "altitudestate.h" @@ -15,6 +9,11 @@ #include "positionstate.h" #include "gcstelemetrystats.h" +#include +#include +#include +#include + class Simulator : public QObject { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index c77b26f91..355e76448 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -27,11 +27,13 @@ #include "simulator.h" -#include "extensionsystem/pluginmanager.h" -#include "coreplugin/icore.h" -#include "coreplugin/threadmanager.h" #include "hitlnoisegeneration.h" +#include +#include +#include +#include + volatile bool Simulator::isStarted = false; const float Simulator::GEE = 9.81; diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index 7d9ac4ad9..8eb24ff5f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -28,14 +28,7 @@ #ifndef ISIMULATOR_H #define ISIMULATOR_H -#include -#include -#include -#include -#include - #include "qscopedpointer.h" -#include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" #include "accelstate.h" @@ -61,6 +54,13 @@ #include "utils/coordinateconversions.h" +#include +#include +#include +#include +#include +#include + /** * just imagine this was a class without methods and all public properties */ diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp index bd2a08ade..898e22802 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp @@ -29,15 +29,17 @@ #include "notificationitem.h" #include "notifypluginoptionspage.h" #include "notifylogging.h" + +#include #include +#include + +#include + #include #include #include -#include - -#include - static const QString VERSION = "1.0.0"; // #define DEBUG_NOTIFIES @@ -125,7 +127,7 @@ void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* confi void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj) { - telMngr = qobject_cast(obj); + TelemetryManager *telMngr = qobject_cast(obj); if (telMngr) { connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); } diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h index 39912a94b..8c912fddd 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h @@ -29,7 +29,6 @@ #include #include -#include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" #include "uavobject.h" #include "notificationitem.h" @@ -111,7 +110,6 @@ private: PhononObject phonon; NotifyPluginOptionsPage *mop; - TelemetryManager *telMngr; QMediaPlaylist *playlist; }; diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp index 46f0d6213..8418a1c42 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp @@ -30,7 +30,6 @@ #include "scopegadgetwidget.h" #include "utils/stylehelper.h" -#include "uavtalk/telemetrymanager.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h index a8c4a402d..09351c810 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h @@ -31,7 +31,6 @@ #include #include #include "setupwizard.h" -#include "uavtalk/telemetrymanager.h" #include "abstractwizardpage.h" #include "uploader/enums.h" diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp index 52d6d1f80..fc7232590 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp @@ -25,11 +25,13 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include "systemalarms.h" #include "systemhealthgadgetwidget.h" + #include "utils/stylehelper.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" -#include "systemalarms.h" +#include #include #include diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h index c48526382..42a316ae5 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h @@ -30,7 +30,7 @@ #include "systemhealthgadgetconfiguration.h" #include "uavobject.h" -#include "uavtalk/telemetrymanager.h" + #include #include #include diff --git a/ground/openpilotgcs/src/plugins/telemetry/monitorgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/telemetry/monitorgadgetfactory.cpp index 94faf597a..bdd675458 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/monitorgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/telemetry/monitorgadgetfactory.cpp @@ -25,14 +25,15 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "monitorgadgetfactory.h" -#include "uavtalk/telemetrymanager.h" -#include "extensionsystem/pluginmanager.h" + #include "monitorgadgetconfiguration.h" #include "monitorgadget.h" #include "monitorgadgetoptionspage.h" +#include #include #include +#include MonitorGadgetFactory::MonitorGadgetFactory(QObject *parent) : IUAVGadgetFactory(QString("TelemetryMonitorGadget"), tr("Telemetry Monitor"), parent) diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index ac6951d3a..b9317114c 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -25,9 +25,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "configtaskwidget.h" + +#include +#include "uavsettingsimportexport/uavsettingsimportexportfactory.h" + #include #include -#include "uavsettingsimportexport/uavsettingsimportexportfactory.h" /** * Constructor @@ -324,7 +327,8 @@ void ConfigTaskWidget::forceConnectedState() // dynamic widgets don't recieve th void ConfigTaskWidget::onAutopilotConnect() { if (utilMngr) { - currentBoard = utilMngr->getBoardModel(); // TODO REMEMBER TO ADD THIS TO FORCE CONNECTED FUNC ON CC3D_RELEASE + // TODO REMEMBER TO ADD THIS TO FORCE CONNECTED FUNC ON CC3D_RELEASE + currentBoard = utilMngr->getBoardModel(); } invalidateObjects(); isConnected = true; @@ -1226,8 +1230,8 @@ void ConfigTaskWidget::loadWidgetLimits(QWidget *widget, UAVObjectField *field, void ConfigTaskWidget::updateEnableControls() { TelemetryManager *telMngr = pm->getObject(); - Q_ASSERT(telMngr); + enableControls(telMngr->isConnected()); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.h index 42d9b06e8..68e84cf67 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.h @@ -27,7 +27,6 @@ #ifndef SMARTSAVEBUTTON_H #define SMARTSAVEBUTTON_H -#include "uavtalk/telemetrymanager.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h index 44fcdc944..42e694414 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h @@ -30,17 +30,17 @@ #include "ui_runningdevicewidget.h" -#include -#include -#include -#include -#include "uavtalk/telemetrymanager.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" #include "uavobjectutilmanager.h" #include "uploader_global.h" +#include +#include +#include +#include + class UPLOADER_EXPORT RunningDeviceWidget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 2063fea3a..d6f12e464 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -26,10 +26,13 @@ */ #include "uploadergadgetwidget.h" #include "version_info/version_info.h" -#include -#include #include "flightstatus.h" +#include +#include + +#include + #define DFU_DEBUG true const int UploaderGadgetWidget::AUTOUPDATE_CLOSE_TIMEOUT = 7000; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 3a59de352..14d9ac098 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -36,7 +36,6 @@ #include #include -#include "uavtalk/telemetrymanager.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" From ff14b8d28c7bca2d42aedf2f5243d168288e7678 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 11 Dec 2013 22:24:38 +0100 Subject: [PATCH 058/113] OP-1122 OP-1125 renamed method autoUpdated to instanceAutoUpdated also fixed some method help comments --- flight/uavobjects/uavobjectmanager.c | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 484f948cc..7f229bd2c 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -183,7 +183,7 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask); static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb); -static void autoUpdated(UAVObjHandle obj_handle, uint16_t instId); +static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId); // Private variables static xSemaphoreHandle mutex; @@ -391,8 +391,8 @@ UAVObjHandle UAVObjRegister(uint32_t id, } // fire events for outer object and its embedded meta object - autoUpdated((UAVObjHandle)uavo_data, 0); - autoUpdated((UAVObjHandle) & (uavo_data->metaObj), 0); + instanceAutoUpdated((UAVObjHandle)uavo_data, 0); + instanceAutoUpdated((UAVObjHandle) & (uavo_data->metaObj), 0); unlock_exit: xSemaphoreGiveRecursive(mutex); @@ -1583,8 +1583,8 @@ void UAVObjRequestUpdate(UAVObjHandle obj_handle) } /** - * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event - * will be generated as soon as the object is updated. + * Request an update of the object's data from the GCS. + * The call will not wait for the response, a EV_UPDATED event will be generated as soon as the object is updated. * \param[in] obj The object handle * \param[in] instId Object instance ID to update */ @@ -1597,7 +1597,7 @@ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId) } /** - * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). + * Trigger a EV_UPDATED_MANUAL event for an object. * \param[in] obj The object handle */ void UAVObjUpdated(UAVObjHandle obj_handle) @@ -1606,7 +1606,7 @@ void UAVObjUpdated(UAVObjHandle obj_handle) } /** - * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). + * Trigger a EV_UPDATED_MANUAL event for an object instance. * \param[in] obj The object handle * \param[in] instId The object instance ID */ @@ -1619,12 +1619,11 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId) } /** -<<<<<<< HEAD - * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). + * Trigger a EV_UPDATED event for an object instance. * \param[in] obj The object handle * \param[in] instId The object instance ID */ -static void autoUpdated(UAVObjHandle obj_handle, uint16_t instId) +static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId) { PIOS_Assert(obj_handle); xSemaphoreTakeRecursive(mutex, portMAX_DELAY); @@ -1760,7 +1759,7 @@ static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId) ((struct UAVOMulti *)obj)->num_instances++; // Fire event - autoUpdated((UAVObjHandle)obj, instId); + instanceAutoUpdated((UAVObjHandle)obj, instId); // Done return InstanceDataOffset(instEntry); From 9a24f12235a2ffb787dc074dd331b41fec848785 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 11 Dec 2013 22:42:27 +0100 Subject: [PATCH 059/113] OP-1122 OP-1125 fixed object connection leak in TelemetryMonitor At each board connection a new connection would be added to the firmwareIAPObj This would also cause some code to be later signaled mutliple times on board connection --- ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp index 8342d5001..8ce52e2bd 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp @@ -179,6 +179,7 @@ void TelemetryMonitor::firmwareIAPUpdated(UAVObject *obj) QMutexLocker locker(mutex); if (firmwareIAPObj->getBoardType() != 0) { + disconnect(firmwareIAPObj); emit connected(); } } From bbdef443448a44206a738e21aea7fd0299eb1f1c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 11 Dec 2013 23:13:32 +0100 Subject: [PATCH 060/113] OP-112 OP-1120 waypoint editor - added popup notification for success or failure of uploading/downloading a flight path to the board + minor header file reorg --- .../src/plugins/opmap/modeluavoproxy.cpp | 16 +++++++++------- .../src/plugins/opmap/modeluavoproxy.h | 16 +++++++++------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 6937ee1b0..5af2a707f 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -66,10 +66,10 @@ void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) if (completionCount == 2) { qDebug() << "ModelUavoProxy::flightPlanSent - success" << (completionSuccessCount == 2); if (completionSuccessCount == 2) { - // TODO : popup? + QMessageBox::information(NULL, tr("Flight Plan Upload Successful"), tr("Flight plan upload was successful.")); } else { - // TODO : popup? + QMessageBox::critical(NULL, tr("Flight Plan Upload Failed !"), tr("Failed to upload the flight plan !")); } } } @@ -102,10 +102,10 @@ void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) qDebug() << "ModelUavoProxy::flightPlanReceived - success" << (completionSuccessCount == 2); if (completionSuccessCount == 2) { objectsToModel(); - // TODO : popup? + QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); } else { - // TODO : popup? + QMessageBox::critical(NULL, tr("Flight Plan Download Failed !"), tr("Failed to download the flight plan !")); } } } @@ -254,8 +254,11 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD void ModelUavoProxy::objectsToModel() { + // build model from uav objects + // the list of objects can end with "garbage" instances due to previous flightpath + // they need to be ignored + int instanceCount = objManager->getNumInstances(Waypoint::OBJID); - // TODO retain only reachable waypoints int rowCount = myModel->rowCount(); if (instanceCount < rowCount) { @@ -275,8 +278,7 @@ void ModelUavoProxy::objectsToModel() Waypoint::DataFields waypointData = waypoint->getData(); waypointToModel(i, waypointData); - int actionId = waypointData.Action; - PathAction *action = PathAction::GetInstance(objManager, actionId); + PathAction *action = PathAction::GetInstance(objManager, waypoint->getAction()); Q_ASSERT(action); if (!action) { continue; diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 1b685a739..9e3a2cd2e 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -38,9 +38,6 @@ class ModelUavoProxy : public QObject { public: explicit ModelUavoProxy(QObject *parent, flightDataModel *model); - void modelToObjects(); - void objectsToModel(); - public slots: void sendFlightPlan(); void receiveFlightPlan(); @@ -51,16 +48,21 @@ private: uint completionCount; uint completionSuccessCount; + void modelToObjects(); + void objectsToModel(); + Waypoint *createWaypoint(int index, Waypoint *newWaypoint); - void modelToWaypoint(int i, Waypoint::DataFields &data); - void waypointToModel(int i, Waypoint::DataFields &data); + PathAction *createPathAction(int index, PathAction *newAction); PathAction *findPathAction(const PathAction::DataFields& actionFields, int actionCount); - PathAction *createPathAction(int index, PathAction *newAction); + + void modelToWaypoint(int i, Waypoint::DataFields &data); void modelToPathAction(int i, PathAction::DataFields &data); + + void waypointToModel(int i, Waypoint::DataFields &data); void pathActionToModel(int i, PathAction::DataFields &data); -public slots: +private slots: void flightPlanSent(UAVObject *, bool success); void flightPlanReceived(UAVObject *, bool success); From c3506d07addc61a1e738b81e68ef4059d68be7f1 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 11 Dec 2013 23:56:04 +0100 Subject: [PATCH 061/113] OP-112 OP-1120 bring waypoint editor to the front it case it was open and hidden away --- .../openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index c8b193a84..43e7ac02b 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -502,7 +502,8 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addAction(wayPointEditorAct); contextMenu.addAction(addWayPointActFromContextMenu); - if (m_mouse_waypoint) { // we have a waypoint under the mouse + if (m_mouse_waypoint) { + // we have a waypoint under the mouse contextMenu.addAction(editWayPointAct); lockWayPointAct->setChecked(waypoint_locked); @@ -1870,8 +1871,12 @@ void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action) void OPMapGadgetWidget::onOpenWayPointEditorAct_triggered() { + // open dialog table->show(); + // bring dialog to the front in case it was already open and hidden away + table->raise(); } + void OPMapGadgetWidget::onAddWayPointAct_triggeredFromContextMenu() { onAddWayPointAct_triggered(m_context_menu_lat_lon); From df7db8a01adcb1c17ab432d7d17cbd3eb5b87e7b Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 11 Dec 2013 23:57:27 +0100 Subject: [PATCH 062/113] OP-1122 OP-1125 removed spurious qDebug() calls --- .../src/plugins/uavobjectutil/uavobjectutilmanager.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index a8acfc752..ebcdcecda 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -256,11 +256,9 @@ int UAVObjectUtilManager::getBoardModel() { FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - qDebug() << "Board type=" << firmwareIapData.BoardType; - qDebug() << "Board revision=" << firmwareIapData.BoardRevision; int ret = firmwareIapData.BoardType << 8; ret = ret + firmwareIapData.BoardRevision; - qDebug() << "Board info=" << ret; + return ret; } From 625d7d53fcb852f3592694418428be6e06a96571 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 12 Dec 2013 00:05:58 +0100 Subject: [PATCH 063/113] OP-1122 OP-1125 reworked uavtalk encoding/decoding to fix issues found during OPLM testing Packet sizes are now again properly calculated and sent by GCS Made the ground and flight side more symmetric and robust Fixed few error handling issues Replaced UAVTALK_QXTLOG_DEBUG define with qWarning() calls Known issue : GCS sometimes reports bad CRC on messages received over USB connection --- flight/uavtalk/inc/uavtalk_priv.h | 27 +-- flight/uavtalk/uavtalk.c | 119 +++++----- .../src/plugins/uavtalk/telemetry.cpp | 3 +- .../src/plugins/uavtalk/telemetrymanager.cpp | 8 +- .../src/plugins/uavtalk/uavtalk.cpp | 209 ++++++++++-------- .../src/plugins/uavtalk/uavtalk.h | 13 +- .../src/plugins/uavtalk/uavtalk.pro | 4 +- 7 files changed, 196 insertions(+), 187 deletions(-) diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index a5b148440..9225179d9 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -32,32 +32,21 @@ #include "uavobjectsinit.h" // Private types and constants -typedef struct { - uint8_t sync; - uint8_t type; - uint16_t size; - uint32_t objId; -} uavtalk_min_header; -#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header) -typedef struct { - uint8_t sync; - uint8_t type; - uint16_t size; - uint32_t objId; - uint16_t instId; - uint16_t timestamp; -} uavtalk_max_header; -#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header) +// min header : sync(1), type (1), size(2), object ID(4), instance ID(2) +#define UAVTALK_MIN_HEADER_LENGTH 10 + +// max header : sync(1), type (1), size(2), object ID(4), instance ID(2), timestamp(2) +#define UAVTALK_MAX_HEADER_LENGTH 12 + +#define UAVTALK_CHECKSUM_LENGTH 1 -typedef uint8_t uavtalk_checksum; -#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) #define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1) + #define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH #define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH typedef struct { - UAVObjHandle obj; uint8_t type; uint16_t packet_size; uint32_t objId; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 743f347c1..848b34e87 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -32,9 +32,6 @@ #include "openpilot.h" #include "uavtalk_priv.h" -// Size of instance ID (2 bytes) -#define UAVTALK_INSTANCE_LENGTH 2 - // Private functions static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout); static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); @@ -351,6 +348,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle case UAVTALK_STATE_SYNC: if (rxbyte != UAVTALK_SYNC_VAL) { + // TODO connection->stats.rxSyncErrors++; break; } @@ -359,6 +357,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->rxPacketLength = 1; + iproc->rxCount = 0; + iproc->state = UAVTALK_STATE_TYPE; break; @@ -368,6 +368,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { + // TODO connection->stats.rxSyncErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } @@ -377,7 +378,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->packet_size = 0; iproc->state = UAVTALK_STATE_SIZE; - iproc->rxCount = 0; break; case UAVTALK_STATE_SIZE: @@ -390,15 +390,16 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->rxCount++; break; } - iproc->packet_size += rxbyte << 8; + iproc->rxCount = 0; - if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size + if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { + // incorrect packet size + connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } - iproc->rxCount = 0; iproc->objId = 0; iproc->state = UAVTALK_STATE_OBJID; break; @@ -409,44 +410,12 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); iproc->objId += rxbyte << (8 * (iproc->rxCount++)); - if (iproc->rxCount < 4) { break; } - - // Search for object. - iproc->obj = UAVObjGetByID(iproc->objId); - - // Determine data length - if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { - iproc->length = 0; - } else { - if (iproc->obj) { - iproc->length = UAVObjGetNumBytes(iproc->obj); - } else { - iproc->length = iproc->packet_size - iproc->rxPacketLength; - } - iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; - } - - // Check length and determine next state - if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { - // packet error - exceeded payload max length - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - // Check the lengths match - if ((iproc->rxPacketLength + UAVTALK_INSTANCE_LENGTH + iproc->timestampLength + iproc->length) != iproc->packet_size) { - // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } + iproc->rxCount = 0; // Message always contain an instance ID - iproc->rxCount = 0; iproc->instId = 0; iproc->state = UAVTALK_STATE_INSTID; break; @@ -457,23 +426,54 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); iproc->instId += rxbyte << (8 * (iproc->rxCount++)); - if (iproc->rxCount < 2) { break; } - iproc->rxCount = 0; - // If there is a timestamp, get it - if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) { - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } - // If there is a payload get it, otherwise receive checksum - else if (iproc->length > 0) { - iproc->state = UAVTALK_STATE_DATA; + UAVObjHandle obj = UAVObjGetByID(iproc->objId); + + // Determine data length + if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { + iproc->length = 0; + iproc->timestampLength = 0; } else { - iproc->state = UAVTALK_STATE_CS; + if (obj) { + iproc->length = UAVObjGetNumBytes(obj); + } else { + iproc->length = iproc->packet_size - iproc->rxPacketLength; + } + iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; + } + + // Check length + if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + // packet error - exceeded payload max length + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + // Check the lengths match + if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { + // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + // Determine next state + if (iproc->type & UAVTALK_TIMESTAMPED) { + // If there is a timestamp get it + iproc->timestamp = 0; + iproc->state = UAVTALK_STATE_TIMESTAMP; + } else { + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } } break; @@ -482,11 +482,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); iproc->timestamp += rxbyte << (8 * (iproc->rxCount++)); - if (iproc->rxCount < 2) { break; } - iproc->rxCount = 0; // If there is a payload get it, otherwise receive checksum @@ -506,28 +504,30 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle if (iproc->rxCount < iproc->length) { break; } + iproc->rxCount = 0; iproc->state = UAVTALK_STATE_CS; - iproc->rxCount = 0; break; case UAVTALK_STATE_CS: // the CRC byte - if (rxbyte != iproc->cs) { // packet error - faulty CRC + if (rxbyte != iproc->cs) { + // packet error - faulty CRC connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } - if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size + if (iproc->rxPacketLength != (iproc->packet_size + 1)) { + // packet error - mismatched packet size connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } - connection->stats.rxObjectBytes += iproc->length; connection->stats.rxObjects++; + connection->stats.rxObjectBytes += iproc->length; iproc->state = UAVTALK_STATE_COMPLETE; break; @@ -852,9 +852,8 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) for (n = 0; n < numInst; ++n) { - sendSingleObject(connection, type, objId, numInst - n - 1, obj); - if (UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) { - // TODO check semaphore and bail out if necessary + if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) < 0) { + return -1; } } return 0; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 499384491..67de7b443 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -52,6 +52,7 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMng connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); // Listen to transaction completions + // TODO should send a status (SUCCESS, FAILED, TIMEOUT) connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Get GCS stats object @@ -392,7 +393,7 @@ void Telemetry::processObjectQueue() // If a single instance transaction is running, then starting an "all instance" transaction is not allowed // TODO make the above logic a reality... if (findTransaction(objInfo.obj)) { - qWarning() << "Telemetry - !!! Making request for a object " << objInfo.obj->toStringBrief() << " for which a request is already in progress"; + qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress"; //objInfo.obj->emitTransactionCompleted(false); return; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp index 46e369717..ad3b3db16 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp @@ -59,10 +59,7 @@ void TelemetryManager::start(QIODevice *dev) void TelemetryManager::onStart() { - utalk = new UAVTalk(device, objMngr); - telemetry = new Telemetry(utalk, objMngr); - telemetryMon = new TelemetryMonitor(objMngr, telemetry); - + utalk = new UAVTalk(device, objMngr); if (false) { // UAVTalk must be thread safe and for that: // 1- all public methods must lock a mutex @@ -85,6 +82,9 @@ void TelemetryManager::onStart() connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream())); } + telemetry = new Telemetry(utalk, objMngr); + telemetryMon = new TelemetryMonitor(objMngr, telemetry); + connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect())); connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect())); connect(telemetryMon, SIGNAL(telemetryUpdated(double, double)), this, SLOT(onTelemetryUpdate(double, double))); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index d4db0132c..a9f316ff3 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -32,14 +32,9 @@ #include #include -// #define UAVTALK_DEBUG -#ifdef UAVTALK_DEBUG - #define UAVTALK_QXTLOG_DEBUG(args ...) -#else // UAVTALK_DEBUG - #define UAVTALK_QXTLOG_DEBUG(args ...) -#endif // UAVTALK_DEBUG - -#define VERBOSE_FILTER(objId) //if (objId == 0xD23852DC) +#ifdef VERBOSE_UAVTALK +#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC || objId == 0x2472A0AE) +#endif #define SYNC_VAL 0x3C @@ -62,7 +57,6 @@ const quint8 UAVTalk::crc_table[256] = { 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; - /** * Constructor */ @@ -261,7 +255,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxbyte != SYNC_VAL) { // continue until sync byte is matched - UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")"); + // TODO stats.rxSyncErrror++; break; } @@ -270,13 +264,15 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxPacketLength = 1; + // case local byte counter, don't forget to zero it after use. + rxCount = 0; + if (useUDPMirror) { rxDataArray.clear(); rxDataArray.append(rxbyte); } rxState = STATE_TYPE; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type"); break; case STATE_TYPE: @@ -285,8 +281,9 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxCS = updateCRC(rxCS, rxbyte); if ((rxbyte & TYPE_MASK) != TYPE_VER) { + qWarning() << "UAVTalk - error : bad type"; + // TODO stats.rxSyncErrror++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync"); break; } @@ -295,8 +292,6 @@ bool UAVTalk::processInputByte(quint8 rxbyte) packetSize = 0; rxState = STATE_SIZE; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size"); - rxCount = 0; break; case STATE_SIZE: @@ -307,21 +302,21 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCount == 0) { packetSize += rxbyte; rxCount++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size"); break; } - packetSize += (quint32)rxbyte << 8; + rxCount = 0; - if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { // incorrect packet size + + if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { + // incorrect packet size + qWarning() << "UAVTalk - error : incorrect packet size"; + stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync"); break; } - rxCount = 0; rxState = STATE_OBJID; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID"); break; case STATE_OBJID: @@ -331,18 +326,37 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxTmpBuffer[rxCount++] = rxbyte; if (rxCount < 4) { - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID"); break; } + rxCount = 0; + + rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); + + // Message always contain an instance ID + rxInstId = 0; + rxState = STATE_INSTID; + break; + + case STATE_INSTID: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount < 2) { + break; + } + rxCount = 0; + + rxInstId = (qint16)qFromLittleEndian(rxTmpBuffer); // Search for object, if not found reset state machine - rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); { UAVObject *rxObj = objMngr->getObject(rxObjId); if (rxObj == NULL && rxType != TYPE_OBJ_REQ) { + qWarning() << "UAVTalk - error : missing object" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)"); break; } @@ -360,50 +374,28 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // Check length and determine next state if (rxLength >= MAX_PAYLOAD_LENGTH) { // packet error - exceeded payload max length + qWarning() << "UAVTalk - error : exceeded payload max length" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); break; } // Check the lengths match - if ((rxPacketLength + INSTANCE_LENGTH + rxLength) != packetSize) { + if ((rxPacketLength + rxLength) != packetSize) { // packet error - mismatched packet size + qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)"); break; } - // Message always contain an instance ID - rxCount = 0; - rxInstId = 0; - rxState = STATE_INSTID; } - break; - - case STATE_INSTID: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxTmpBuffer[rxCount++] = rxbyte; - if (rxCount < 2) { - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID"); - break; - } - - rxInstId = (qint16)qFromLittleEndian(rxTmpBuffer); - - rxCount = 0; // If there is a payload get it, otherwise receive checksum if (rxLength > 0) { rxState = STATE_DATA; - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data"); } else { rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum"); } break; @@ -414,13 +406,11 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxBuffer[rxCount++] = rxbyte; if (rxCount < rxLength) { - UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data"); break; } + rxCount = 0; rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum"); - rxCount = 0; break; case STATE_CS: @@ -430,17 +420,17 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCS != rxCSPacket) { // packet error - faulty CRC + qWarning() << "UAVTalk - error : faulty CRC" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)"); break; } if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size + qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)"); break; } @@ -458,14 +448,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) } rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)"); break; default: - rxState = STATE_SYNC; + qWarning() << "UAVTalk - error : bad state"; stats.rxErrors++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered. + rxState = STATE_SYNC; } // Done @@ -491,6 +480,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length) { Q_UNUSED(length); + UAVObject *obj = NULL; bool error = false; bool allInstances = (instId == ALL_INSTANCES); @@ -603,7 +593,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * error = true; } if (error) { - qWarning() << "UAVTalk - !!! error receiving object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + qWarning() << "UAVTalk - !!! error receiving" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); } // Done return !error; @@ -624,19 +614,18 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data) // Get the object type UAVObject *typeObj = objMngr->getObject(objId); if (typeObj == NULL) { - qWarning() << "UAVTalk - Failed to get object, object ID :" << objId; + qWarning() << "UAVTalk - failed to get object, object ID :" << objId; return NULL; } // Make sure this is a data object UAVDataObject *dataObj = dynamic_cast(typeObj); if (dataObj == NULL) { - qWarning() << "UAVTalk - Object is not a data object, object ID :" << objId; return NULL; } // Create a new instance, unpack and register UAVDataObject *instObj = dataObj->clone(instId); if (!objMngr->registerObject(instObj)) { - qWarning() << "UAVTalk - Failed to register object " << instObj->toStringBrief(); + qWarning() << "UAVTalk - failed to register object " << instObj->toStringBrief(); return NULL; } instObj->unpack(data); @@ -710,46 +699,44 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje } bool allInstances = (instId == ALL_INSTANCES); +#ifdef VERBOSE_UAVTALK + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif + // Process message type + bool ret = false; if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { if (allInstances) { // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) + ret = true; quint32 numInst = objMngr->getNumInstances(objId); for (quint32 n = 0; n < numInst; ++n) { quint32 i = numInst - n - 1; - UAVObject *o = objMngr->getObject(objId, i); if (!transmitSingleObject(type, objId, i, o)) { - return false; - } - - if (false) { - // yield - mutex.unlock(); - // going back to the event loop is necessary to allow timeout events to be fired - // but don't allow user events as the event can cause the main thread to reenter UAVTalk - // the timer event suffers from the same issue but this case is handled - //QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents, 10); - QThread::msleep(1); - mutex.lock(); + ret = false; + break; } } - return true; } else { - return transmitSingleObject(type, objId, instId, obj); + ret = transmitSingleObject(type, objId, instId, obj); } } else if (type == TYPE_OBJ_REQ) { - return transmitSingleObject(TYPE_OBJ_REQ, objId, instId, obj); + ret = transmitSingleObject(TYPE_OBJ_REQ, objId, instId, NULL); } else if (type == TYPE_ACK || type == TYPE_NACK) { if (!allInstances) { - return transmitSingleObject(type, objId, instId, obj); - } else { - return false; + ret = transmitSingleObject(type, objId, instId, NULL); } - } else { - return false; } + +#ifdef VERBOSE_UAVTALK + if (!ret) { + VERBOSE_FILTER(objId) qDebug() << "UAVTalk - failed to transmit" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); + } +#endif + + return ret; } /** @@ -763,11 +750,6 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { qint32 length; - qint32 dataOffset; - -#ifdef VERBOSE_UAVTALK - VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); -#endif // IMPORTANT : obj can be null (when type is NACK for example) @@ -780,7 +762,6 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U qToLittleEndian(objId, &txBuffer[4]); // Setup instance ID qToLittleEndian(instId, &txBuffer[8]); - dataOffset = 10; // Determine data length if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) { @@ -791,37 +772,48 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U // Check length if (length >= MAX_PAYLOAD_LENGTH) { + qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief(); return false; } // Copy data (if any) if (length > 0) { - if (!obj->pack(&txBuffer[dataOffset])) { + if (!obj->pack(&txBuffer[HEADER_LENGTH])) { + qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief(); return false; } } // Store the packet length - qToLittleEndian(dataOffset + length, &txBuffer[2]); + qToLittleEndian(HEADER_LENGTH + length, &txBuffer[2]); // Calculate checksum - txBuffer[dataOffset + length] = updateCRC(0, txBuffer, dataOffset + length); + txBuffer[HEADER_LENGTH + length] = updateCRC(0, txBuffer, HEADER_LENGTH + length); // Send buffer, check that the transmit backlog does not grow above limit - if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) { - io->write((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH); - if (useUDPMirror) { - udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); + if (!io.isNull() && io->isWritable()) { + if (io->bytesToWrite() < TX_BUFFER_SIZE) { + io->write((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH); + if (useUDPMirror) { + udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); + } } + else { + qWarning() << "UAVTalk - error transmitting : io device full"; + ++stats.txErrors; + return false; + } + } else { + qWarning() << "UAVTalk - error transmitting : io device not writable"; ++stats.txErrors; return false; } // Update stats ++stats.txObjects; - stats.txBytes += dataOffset + length + CHECKSUM_LENGTH; stats.txObjectBytes += length; + stats.txBytes += HEADER_LENGTH + length + CHECKSUM_LENGTH; // Done return true; @@ -913,3 +905,30 @@ void UAVTalk::closeAllTransactions() delete objTransactions; } } + +const char* UAVTalk::typeToString(quint8 type) +{ + switch (type) { + case TYPE_OBJ: + return "object"; + break; + + case TYPE_OBJ_ACK: + return "object (acked)"; + break; + + case TYPE_OBJ_REQ: + return "object request"; + break; + + case TYPE_ACK: + return "ack"; + break; + + case TYPE_NACK: + return "nack"; + break; + + } + return ""; +} diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 15f86cdaa..85a769a1b 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -91,18 +91,17 @@ private: static const int TYPE_ACK = (TYPE_VER | 0x03); static const int TYPE_NACK = (TYPE_VER | 0x04); - static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4) - static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects) - - static const int CHECKSUM_LENGTH = 1; + // header : sync(1), type (1), size(2), object ID(4), instance ID(2) + static const int HEADER_LENGTH = 10; static const int MAX_PAYLOAD_LENGTH = 256; - static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); + static const int CHECKSUM_LENGTH = 1; - static const quint8 INSTANCE_LENGTH = 2; + static const int MAX_PACKET_LENGTH = (HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); static const int TX_BUFFER_SIZE = 2 * 1024; + static const quint8 crc_table[256]; // Types @@ -150,6 +149,8 @@ private: void openTransaction(quint8 type, quint32 objId, quint16 instId); void closeTransaction(Transaction *trans); void closeAllTransactions(); + + const char *typeToString(quint8 type); }; #endif // UAVTALK_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro index 59275fa3c..8ec9c6461 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro @@ -5,8 +5,8 @@ QT += network DEFINES += UAVTALK_LIBRARY -#DEFINES += VERBOSE_TELEMETRY -#DEFINES += VERBOSE_UAVTALK +DEFINES += VERBOSE_TELEMETRY +DEFINES += VERBOSE_UAVTALK include(../../openpilotgcsplugin.pri) include(uavtalk_dependencies.pri) From 29df9d6dad71b0b2f7fa641ea8e6c569ec0af59f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 13 Dec 2013 17:10:11 +0100 Subject: [PATCH 064/113] OP-1139: Add some more descriptive names and comments for compensation variables --- flight/pios/common/pios_ms5611.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index f21fff0fc..db8d16f1b 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -51,7 +51,10 @@ static int64_t Temperature; static int32_t lastConversionStart; static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); -static int64_t t2; + +// Second order temperature compensation. Temperature offset +static int64_t compensation_t2; + // Move into proper driver structure with cfg stored static uint32_t oversampling; static const struct pios_ms5611_cfg *dev_cfg; @@ -74,8 +77,8 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) uint8_t data[2]; - // reset calibration values - t2 = 0; + // reset temperature compensation values + compensation_t2 = 0; /* Calibration parameters */ for (int i = 0; i < 6; i++) { @@ -205,7 +208,7 @@ int32_t PIOS_MS5611_ReadADC(void) if (Temperature < 2000) { Offset2 = 5 * (Temperature - 2000) >> 1; Sens2 = Offset2 >> 1; - t2 = (deltaTemp * deltaTemp) >> 31; + compensation_t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C if (Temperature < -1500) { int64_t tcorr = (Temperature + 1500) * (Temperature + 1500); @@ -213,9 +216,9 @@ int32_t PIOS_MS5611_ReadADC(void) Sens2 += (11 * tcorr) >> 1; } } else { - t2 = 0; + compensation_t2 = 0; Offset2 = 0; - Sens2 = 0; + Sens2 = 0; } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; @@ -231,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float)(Temperature - t2)) / 100.0f; + // Apply the second order low and very low temperature compensation offset + return ((float)(Temperature - compensation_t2)) / 100.0f; } /** From f95a86eaed8971761bd78209c7c75166cf037255 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 13 Dec 2013 17:10:39 +0100 Subject: [PATCH 065/113] OP-1139: missing uncrustification --- flight/modules/Altitude/revolution/altitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index d46c7c5bb..0c6dbd2ed 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -163,10 +163,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); - float temp2 = temp * temp; + float temp2 = temp * temp; press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; From 2d1a17e267bfa692dbfbf833d95eed63a9e2c5fb Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 14 Dec 2013 16:02:14 +0100 Subject: [PATCH 066/113] OP-1122 OP-1125 minor uavtalk cleanups - removed some magic numbers - improved some error messages - minor error handling improvments --- flight/uavobjects/uavobjectmanager.c | 8 +- flight/uavtalk/inc/uavtalk_priv.h | 1 - flight/uavtalk/uavtalk.c | 120 ++++++++++-------- .../src/plugins/uavtalk/uavtalk.cpp | 8 +- 4 files changed, 74 insertions(+), 63 deletions(-) diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 7f229bd2c..d350a9d53 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -1678,8 +1678,7 @@ xSemaphoreGiveRecursive(mutex); /** * Send a triggered event to all event queues registered on the object. */ -static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, - UAVObjEventType triggered_event) +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType triggered_event) { /* Set up the message that will be sent to all registered listeners */ UAVObjEvent msg = { @@ -1692,14 +1691,13 @@ static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, struct ObjectEventEntry *event; LL_FOREACH(obj->next_event, event) { - if (event->eventMask == 0 - || (event->eventMask & triggered_event) != 0) { + if (event->eventMask == 0 || (event->eventMask & triggered_event) != 0) { // Send to queue if a valid queue is registered if (event->queue) { // will not block if (xQueueSend(event->queue, &msg, 0) != pdTRUE) { - stats.lastQueueErrorID = UAVObjGetID(obj); ++stats.eventQueueErrors; + stats.lastQueueErrorID = UAVObjGetID(obj); } } diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 9225179d9..e853c6002 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -72,7 +72,6 @@ typedef struct { UAVTalkStats stats; UAVTalkInputProcessor iproc; uint8_t *rxBuffer; - uint32_t txSize; uint8_t *txBuffer; } UAVTalkConnectionData; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 848b34e87..16bf7a029 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -156,7 +156,7 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) statsOut->txObjects += connection->stats.txObjects; statsOut->rxObjects += connection->stats.rxObjects; statsOut->txErrors += connection->stats.txErrors; - statsOut->txErrors += connection->stats.txErrors; + statsOut->rxErrors += connection->stats.rxErrors; // Release lock xSemaphoreGiveRecursive(connection->lock); @@ -195,7 +195,6 @@ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *times *timestamp = iproc->timestamp; } - /** * Request an update for the specified object, on success the object data would have been * updated by the GCS. @@ -519,7 +518,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } - if (iproc->rxPacketLength != (iproc->packet_size + 1)) { + if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) { // packet error - mismatched packet size connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; @@ -595,53 +594,53 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // Setup type outConnection->txBuffer[1] = inIproc->type; // next 2 bytes are reserved for data length (inserted here later) - int32_t dataOffset = 4; - if (inIproc->objId != 0) { - // Setup object ID - outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF); - outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF); - outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF); - outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF); - - // Setup instance ID - outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); - outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); - dataOffset = 10; - } + // Setup object ID + outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF); + outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF); + outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF); + outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF); + // Setup instance ID + outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); + outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); + int32_t headerLength = 10; // Add timestamp when the transaction type is appropriate if (inIproc->type & UAVTALK_TIMESTAMPED) { portTickType time = xTaskGetTickCount(); - outConnection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - outConnection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; + outConnection->txBuffer[10] = (uint8_t)(time & 0xFF); + outConnection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF); + headerLength += 2; } // Copy data (if any) - memcpy(&outConnection->txBuffer[dataOffset], inConnection->rxBuffer, inIproc->length); + if (inIproc->length > 0) { + memcpy(&outConnection->txBuffer[headerLength], inConnection->rxBuffer, inIproc->length); + } // Store the packet length - outConnection->txBuffer[2] = (uint8_t)((dataOffset + inIproc->length) & 0xFF); - outConnection->txBuffer[3] = (uint8_t)(((dataOffset + inIproc->length) >> 8) & 0xFF); + outConnection->txBuffer[2] = (uint8_t)((headerLength + inIproc->length) & 0xFF); + outConnection->txBuffer[3] = (uint8_t)(((headerLength + inIproc->length) >> 8) & 0xFF); // Copy the checksum - outConnection->txBuffer[dataOffset + inIproc->length] = inIproc->cs; + outConnection->txBuffer[headerLength + inIproc->length] = inIproc->cs; // Send the buffer. - int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, inIproc->rxPacketLength); + int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH); // Update stats outConnection->stats.txBytes += rc; + // evaluate return value before releasing the lock + UAVTalkRxState rxState = 0; + if (rc != (int32_t) (headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) { + rxState = -1; + } + // Release lock xSemaphoreGiveRecursive(outConnection->lock); // Done - if (rc != inIproc->rxPacketLength) { - return -1; - } - - return 0; + return rxState; } /** @@ -712,8 +711,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // Lock xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - // Get the handle to the Object. - // Will be zero if object does not exist. + // Get the handle to the object. Will be null if object does not exist. // Warning : // Here we ask for instance ID 0 without taking into account the provided instId // The provided instId will be used later when packing, unpacking, etc... @@ -724,7 +722,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, switch (type) { case UAVTALK_TYPE_OBJ: case UAVTALK_TYPE_OBJ_TS: - // All instances, not allowed for OBJ messages + // All instances not allowed for OBJ messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! if (UAVObjUnpack(obj, instId, data) == 0) { @@ -743,7 +741,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, case UAVTALK_TYPE_OBJ_ACK: case UAVTALK_TYPE_OBJ_ACK_TS: - // All instances, not allowed for OBJ_ACK messages + // All instances not allowed for OBJ_ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! if (UAVObjUnpack(obj, instId, data) == 0) { @@ -788,7 +786,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, break; case UAVTALK_TYPE_ACK: - // All instances, not allowed for ACK messages + // All instances not allowed for ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Check if an ACK is pending updateAck(connection, type, objId, instId); @@ -816,9 +814,16 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, */ static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId) { - if ((connection->respType == type) && (connection->respObjId == objId) && ((connection->respInstId == instId) || (connection->respInstId == UAVOBJ_ALL_INSTANCES))) { - xSemaphoreGive(connection->respSema); - connection->respObjId = 0; + if ((connection->respObjId == objId) && (connection->respType == type)) { + if ((connection->respInstId == UAVOBJ_ALL_INSTANCES) && (instId == 0)) { + // last instance received, complete transaction + xSemaphoreGive(connection->respSema); + connection->respObjId = 0; + } + else if (connection->respInstId == instId) { + xSemaphoreGive(connection->respSema); + connection->respObjId = 0; + } } } @@ -852,7 +857,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) for (n = 0; n < numInst; ++n) { - if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) < 0) { + if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) { return -1; } } @@ -886,9 +891,6 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 */ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj) { - int32_t length; - int32_t dataOffset; - // IMPORTANT : obj can be null (when type is NACK for example) if (!connection->outStream) { @@ -908,17 +910,18 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, // Setup instance ID connection->txBuffer[8] = (uint8_t)(instId & 0xFF); connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset = 10; + int32_t headerLength = 10; // Add timestamp when the transaction type is appropriate if (type & UAVTALK_TIMESTAMPED) { portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; + connection->txBuffer[10] = (uint8_t)(time & 0xFF); + connection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF); + headerLength += 2; } // Determine data length + int32_t length; if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) { length = 0; } else { @@ -926,32 +929,41 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, } // Check length - if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + if (length > UAVOBJECTS_LARGEST) { + connection->stats.txErrors++; return -1; } // Copy data (if any) if (length > 0) { - if (UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0) { + if (UAVObjPack(obj, instId, &connection->txBuffer[headerLength]) == -1) { + connection->stats.txErrors++; return -1; } } // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset + length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset + length) >> 8) & 0xFF); + connection->txBuffer[2] = (uint8_t)((headerLength + length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((headerLength + length) >> 8) & 0xFF); - // Calculate checksum - connection->txBuffer[dataOffset + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset + length); + // Calculate and store checksum + connection->txBuffer[headerLength + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, headerLength + length); - uint16_t tx_msg_len = dataOffset + length + UAVTALK_CHECKSUM_LENGTH; + // Send object + uint16_t tx_msg_len = headerLength + length + UAVTALK_CHECKSUM_LENGTH; int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + // Update stats if (rc == tx_msg_len) { - // Update stats ++connection->stats.txObjects; - connection->stats.txBytes += tx_msg_len; connection->stats.txObjectBytes += length; + connection->stats.txBytes += tx_msg_len; + } + else { + connection->stats.txErrors++; + // TDOD rc == -1 connection not open, -2 buffer full should retry + connection->stats.txBytes += (rc > 0) ? rc : 0; + return -1; } // Done diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index a9f316ff3..e8a67f53d 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -354,7 +354,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) { UAVObject *rxObj = objMngr->getObject(rxObjId); if (rxObj == NULL && rxType != TYPE_OBJ_REQ) { - qWarning() << "UAVTalk - error : missing object" << rxObjId; + qWarning() << "UAVTalk - error : unknown object" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; break; @@ -420,13 +420,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCS != rxCSPacket) { // packet error - faulty CRC - qWarning() << "UAVTalk - error : faulty CRC" << rxObjId; + qWarning() << "UAVTalk - error : failed CRC check" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; break; } - if (rxPacketLength != packetSize + 1) { + if (rxPacketLength != packetSize + CHECKSUM_LENGTH) { // packet error - mismatched packet size qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId; stats.rxErrors++; @@ -773,6 +773,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U // Check length if (length >= MAX_PAYLOAD_LENGTH) { qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief(); + ++stats.txErrors; return false; } @@ -780,6 +781,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U if (length > 0) { if (!obj->pack(&txBuffer[HEADER_LENGTH])) { qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief(); + ++stats.txErrors; return false; } } From f105ca4d41ac80d1c69bc47748e06f871ecdd728 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 14 Dec 2013 16:02:50 +0100 Subject: [PATCH 067/113] OP-1122 OP-1125 commented out uavtalk verbose mode defines --- ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro index 8ec9c6461..59275fa3c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro @@ -5,8 +5,8 @@ QT += network DEFINES += UAVTALK_LIBRARY -DEFINES += VERBOSE_TELEMETRY -DEFINES += VERBOSE_UAVTALK +#DEFINES += VERBOSE_TELEMETRY +#DEFINES += VERBOSE_UAVTALK include(../../openpilotgcsplugin.pri) include(uavtalk_dependencies.pri) From d668153b6d60eb323c25071180993c5b5a09fb10 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 14 Dec 2013 16:03:42 +0100 Subject: [PATCH 068/113] OP-1122 OP-1125 fixed copy/paste error in rfm22b flight code --- flight/pios/common/pios_rfm22b.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index c663da70d..c9ea2bd35 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -1221,7 +1221,7 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio // Something went fairly seriously wrong rfm22b_dev->errors++; } - portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE)); + portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken1 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE)); } else { // Store the event. if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) { From c2e8d25319221373a3f355a364bd37739af56bc6 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 18 Dec 2013 08:52:52 +0100 Subject: [PATCH 069/113] OP-1145 made MetaObjectId define publicly available --- flight/uavobjects/inc/uavobjectmanager.h | 2 ++ flight/uavobjects/uavobjectmanager.c | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index 349d7c3dd..9da465cba 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/flight/uavobjects/inc/uavobjectmanager.h @@ -49,6 +49,8 @@ typedef void *UAVObjHandle; +#define MetaObjectId(id) ((id) + 1) + /** * Object update mode, used by multiple modules (e.g. telemetry and logger) */ diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index d350a9d53..2a02b3ee1 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -167,7 +167,6 @@ struct UAVOMulti { #define MetaObjectPtr(obj) ((struct UAVODataMeta *)&((obj)->metaObj)) #define MetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->instance0)) #define LinkedMetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->metaObj.instance0)) -#define MetaObjectId(id) ((id) + 1) /** all information about instances are dependant on object type **/ #define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0))) From 5f33fadb1b6bf325bd1120dc6c2c38aafb5a4c58 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 19 Dec 2013 00:17:08 +0100 Subject: [PATCH 070/113] OP-1122 OP-1145 flight side uavtalk and telemetry enhancements - added more stats (bytes, sync errors, crc errors) - made error handling more robust - added some optional PIOS_DEBUGLOG calls - fixed a timestamp handling bug --- flight/modules/Telemetry/telemetry.c | 52 +++++--- flight/uavtalk/inc/uavtalk.h | 10 +- flight/uavtalk/inc/uavtalk_priv.h | 3 +- flight/uavtalk/uavtalk.c | 119 ++++++++++-------- .../flighttelemetrystats.xml | 12 +- 5 files changed, 118 insertions(+), 78 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index c43cddff7..78a498340 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -179,7 +179,6 @@ static void registerObject(UAVObjHandle obj) if (UAVObjIsMetaobject(obj)) { /* Only connect change notifications for meta objects. No periodic updates */ UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); - return; } else { // Setup object for periodic updates UAVObjEvent ev = { @@ -191,7 +190,6 @@ static void registerObject(UAVObjHandle obj) ev.event = EV_LOGGING_PERIODIC; EventPeriodicQueueCreate(&ev, queue, 0); - // Setup object for telemetry updates updateObject(obj, EV_NONE); } @@ -208,9 +206,8 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) int32_t eventMask; if (UAVObjIsMetaobject(obj)) { - /* This function updates the periodic updates for the object. - * Meta Objects cannot have periodic updates. - */ + // This function updates the periodic updates for the object. + // Meta Objects cannot have periodic updates. PIOS_Assert(false); return; } @@ -318,29 +315,36 @@ static void processObjEvent(UAVObjEvent *ev) if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout - ++retries; + // call blocks until ack is received or timeout + success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); + if (success == -1) { + ++retries; + } } // Update stats - txRetries += (retries - 1); + txRetries += retries; if (success == -1) { ++txErrors; } } else if (ev->event == EV_UPDATE_REQ) { // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout - ++retries; + // call blocks until update is received or timeout + success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); + if (success == -1) { + ++retries; + } } // Update stats - txRetries += (retries - 1); + txRetries += retries; if (success == -1) { ++txErrors; } } // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { - updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for + // linked object will be the actual object the metadata are for + updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); } else { if (updateMode == UPDATEMODE_THROTTLED) { // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. @@ -553,22 +557,30 @@ static void updateTelemetryStats() // Update stats object if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.RxFailures += utalkStats.rxErrors; + flightStats.TxBytes += utalkStats.txBytes; flightStats.TxFailures += txErrors; flightStats.TxRetries += txRetries; - txErrors = 0; - txRetries = 0; + + flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.RxBytes += utalkStats.rxBytes; + flightStats.RxFailures += utalkStats.rxErrors; + flightStats.RxSyncErrors += utalkStats.rxSyncErrors; + flightStats.RxCrcErrors += utalkStats.rxCrcErrors; } else { - flightStats.RxDataRate = 0; flightStats.TxDataRate = 0; - flightStats.RxFailures = 0; + flightStats.TxBytes = 0; flightStats.TxFailures = 0; flightStats.TxRetries = 0; - txErrors = 0; - txRetries = 0; + + flightStats.RxDataRate = 0; + flightStats.RxBytes = 0; + flightStats.RxFailures = 0; + flightStats.RxSyncErrors = 0; + flightStats.RxCrcErrors = 0; } + txErrors = 0; + txRetries = 0; // Check for connection timeout timeNow = xTaskGetTickCount() * portTICK_RATE_MS; diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index b3a638bfc..9a346be3a 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -34,13 +34,17 @@ typedef int32_t (*UAVTalkOutputStream)(uint8_t *data, int32_t length); typedef struct { uint32_t txBytes; - uint32_t rxBytes; uint32_t txObjectBytes; - uint32_t rxObjectBytes; - uint32_t rxObjects; uint32_t txObjects; uint32_t txErrors; + + uint32_t rxBytes; + uint32_t rxObjectBytes; + uint32_t rxObjects; uint32_t rxErrors; + uint32_t rxSyncErrors; + uint32_t rxCrcErrors; + } UAVTalkStats; typedef void *UAVTalkConnection; diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index e853c6002..c7cab8d9e 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -76,9 +76,8 @@ typedef struct { } UAVTalkConnectionData; #define UAVTALK_CANARI 0xCA -#define UAVTALK_WAITFOREVER -1 -#define UAVTALK_NOWAIT 0 #define UAVTALK_SYNC_VAL 0x3C + #define UAVTALK_TYPE_MASK 0x78 #define UAVTALK_TYPE_VER 0x20 #define UAVTALK_TIMESTAMPED 0x80 diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 16bf7a029..7e49344de 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -32,11 +32,21 @@ #include "openpilot.h" #include "uavtalk_priv.h" +//#define UAV_DEBUGLOG 1 + +#if defined UAV_DEBUGLOG && defined(FLASH_FREETOS) +#define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__) +#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == 0x5E5903CC) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } +#else +#define UAVT_DEBUGLOG_PRINTF(...) +#define UAVT_DEBUGLOG_CPRINTF(objId, ...) +#endif + // Private functions static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout); static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length); +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data); static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId); /** @@ -150,13 +160,15 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) // Copy stats statsOut->txBytes += connection->stats.txBytes; - statsOut->rxBytes += connection->stats.rxBytes; statsOut->txObjectBytes += connection->stats.txObjectBytes; - statsOut->rxObjectBytes += connection->stats.rxObjectBytes; statsOut->txObjects += connection->stats.txObjects; - statsOut->rxObjects += connection->stats.rxObjects; statsOut->txErrors += connection->stats.txErrors; + statsOut->rxBytes += connection->stats.rxBytes; + statsOut->rxObjectBytes += connection->stats.rxObjectBytes; + statsOut->rxObjects += connection->stats.rxObjects; statsOut->rxErrors += connection->stats.rxErrors; + statsOut->rxSyncErrors += connection->stats.rxSyncErrors; + statsOut->rxCrcErrors += connection->stats.rxCrcErrors; // Release lock xSemaphoreGiveRecursive(connection->lock); @@ -278,7 +290,7 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs) { int32_t respReceived; - + int32_t ret = -1; // Send object depending on if a response is needed if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) { // Get transaction lock (will block if a transaction is pending) @@ -289,12 +301,19 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK; connection->respObjId = UAVObjGetID(obj); connection->respInstId = instId; - sendObject(connection, type, UAVObjGetID(obj), instId, obj); + ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); - // Wait for response (or timeout) - respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS); + // Wait for response (or timeout) if sending the object succeeded + respReceived = pdFALSE; + if (ret == 0) { + respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS); + } // Check if a response was received - if (respReceived == pdFALSE) { + if (respReceived == pdTRUE) { + // We are done successfully + xSemaphoreGiveRecursive(connection->transLock); + ret = 0; + } else { // Cancel transaction xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); // non blocking call to make sure the value is reset to zero (binary sema) @@ -303,18 +322,13 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->transLock); return -1; - } else { - xSemaphoreGiveRecursive(connection->transLock); - return 0; } } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) { xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - sendObject(connection, type, UAVObjGetID(obj), instId, obj); + ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); - return 0; - } else { - return -1; } + return ret; } /** @@ -347,7 +361,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle case UAVTALK_STATE_SYNC: if (rxbyte != UAVTALK_SYNC_VAL) { - // TODO connection->stats.rxSyncErrors++; + connection->stats.rxSyncErrors++; break; } @@ -355,27 +369,26 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->cs = PIOS_CRC_updateByte(0, rxbyte); iproc->rxPacketLength = 1; - iproc->rxCount = 0; + iproc->type = 0; iproc->state = UAVTALK_STATE_TYPE; break; case UAVTALK_STATE_TYPE: - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { - // TODO connection->stats.rxSyncErrors++; - iproc->state = UAVTALK_STATE_ERROR; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; break; } + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + iproc->type = rxbyte; iproc->packet_size = 0; - iproc->state = UAVTALK_STATE_SIZE; break; @@ -414,7 +427,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } iproc->rxCount = 0; - // Message always contain an instance ID iproc->instId = 0; iproc->state = UAVTALK_STATE_INSTID; break; @@ -437,12 +449,12 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->length = 0; iproc->timestampLength = 0; } else { + iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; if (obj) { iproc->length = UAVObjGetNumBytes(obj); } else { - iproc->length = iproc->packet_size - iproc->rxPacketLength; + iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength; } - iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; } // Check length @@ -477,6 +489,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; case UAVTALK_STATE_TIMESTAMP: + // update the CRC iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); @@ -510,9 +523,11 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle case UAVTALK_STATE_CS: - // the CRC byte + // Check the CRC byte if (rxbyte != iproc->cs) { // packet error - faulty CRC + UAVT_DEBUGLOG_PRINTF("BAD CRC"); + connection->stats.rxCrcErrors++; connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; @@ -532,8 +547,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; default: - connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; } // Done @@ -576,6 +592,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // The input packet must be completely parsed. if (inIproc->state != UAVTALK_STATE_COMPLETE) { + inConnection->stats.rxErrors++; return -1; } @@ -583,13 +600,13 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC CHECKCONHANDLE(outConnectionHandle, outConnection, return -1); if (!outConnection->outStream) { + outConnection->stats.txErrors++; return -1; } // Lock xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY); - // Setup sync byte outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // Setup type outConnection->txBuffer[1] = inIproc->type; @@ -628,11 +645,12 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH); // Update stats - outConnection->stats.txBytes += rc; + outConnection->stats.txBytes += (rc > 0) ? rc : 0; // evaluate return value before releasing the lock UAVTalkRxState rxState = 0; if (rc != (int32_t) (headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) { + outConnection->stats.txErrors++; rxState = -1; } @@ -660,7 +678,7 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) return -1; } - receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer); return 0; } @@ -668,7 +686,6 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) /** * Get the object ID of the current packet. * \param[in] connectionHandle UAVTalkConnection to be used - * \param[in] objId Object ID to send a NACK for * \return The object ID, or 0 on error. */ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) @@ -698,13 +715,7 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) * \return 0 Success * \return -1 Failure */ -static int32_t receiveObject(UAVTalkConnectionData *connection, - uint8_t type, - uint32_t objId, - uint16_t instId, - uint8_t *data, - __attribute__((unused)) int32_t length) -{ +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data) { UAVObjHandle obj; int32_t ret = 0; @@ -741,10 +752,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, case UAVTALK_TYPE_OBJ_ACK: case UAVTALK_TYPE_OBJ_ACK_TS: + UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId); // All instances not allowed for OBJ_ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! if (UAVObjUnpack(obj, instId, data) == 0) { + UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId); // Object updated or created, transmit ACK sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL); } else { @@ -755,21 +768,24 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, } if (ret == -1) { // failed to update object, transmit NACK + UAVT_DEBUGLOG_PRINTF("OBJ NACK %X %d", objId, instId); sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); } break; case UAVTALK_TYPE_OBJ_REQ: // Check if requested object exists - if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) { + UAVT_DEBUGLOG_CPRINTF(objId, "REQ %X %d", objId, instId); + if (obj) { // Object found, transmit it // The sent object will ack the object request on the receiver side - sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj); + ret = sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj); } else { ret = -1; } if (ret == -1) { // failed to send object, transmit NACK + UAVT_DEBUGLOG_PRINTF("REQ NACK %X %d", objId, instId); sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); } break; @@ -841,6 +857,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 { uint32_t numInst; uint32_t n; + uint32_t ret = -1; // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences. @@ -856,26 +873,25 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 numInst = UAVObjGetNumInstances(obj); // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) + ret = 0; for (n = 0; n < numInst; ++n) { if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) { - return -1; + ret = -1; + break; } } - return 0; } else { - return sendSingleObject(connection, type, objId, instId, obj); + ret = sendSingleObject(connection, type, objId, instId, obj); } } else if (type == UAVTALK_TYPE_OBJ_REQ) { - return sendSingleObject(connection, UAVTALK_TYPE_OBJ_REQ, objId, instId, obj); + ret = sendSingleObject(connection, type, objId, instId, obj); } else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) { if (instId != UAVOBJ_ALL_INSTANCES) { - return sendSingleObject(connection, type, objId, instId, obj); - } else { - return -1; + ret = sendSingleObject(connection, type, objId, instId, obj); } - } else { - return -1; } + + return ret; } /** @@ -894,6 +910,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, // IMPORTANT : obj can be null (when type is NACK for example) if (!connection->outStream) { + connection->stats.txErrors++; return -1; } diff --git a/shared/uavobjectdefinition/flighttelemetrystats.xml b/shared/uavobjectdefinition/flighttelemetrystats.xml index 2e371b7de..aa22bb097 100644 --- a/shared/uavobjectdefinition/flighttelemetrystats.xml +++ b/shared/uavobjectdefinition/flighttelemetrystats.xml @@ -1,12 +1,20 @@ Maintains the telemetry statistics from the OpenPilot flight computer. + + - + - + + + + + + + From 02e3970864198edcc9f07ba0b398327945238998 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 19 Dec 2013 03:19:29 +0100 Subject: [PATCH 071/113] OP-1122 OP-1145 ground side uavtalk and telemetry enhancements - added more stats (bytes, sync errors, crc errors) - made error handling more robust --- .../src/plugins/uavtalk/telemetry.cpp | 15 +++++--- .../src/plugins/uavtalk/telemetry.h | 11 +++--- .../src/plugins/uavtalk/telemetrymonitor.cpp | 9 +++-- .../src/plugins/uavtalk/uavtalk.cpp | 14 ++++---- .../src/plugins/uavtalk/uavtalk.h | 36 +++++++++++++------ .../uavobjectdefinition/gcstelemetrystats.xml | 10 ++++-- 6 files changed, 65 insertions(+), 30 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 67de7b443..6ce88acbe 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -377,7 +377,9 @@ void Telemetry::processObjectQueue() GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) { objQueue.clear(); - if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID) { + if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) && + (objInfo.obj->getObjID() != OPLinkSettings::OBJID) && + (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) { objInfo.obj->emitTransactionCompleted(false); return; } @@ -498,15 +500,18 @@ Telemetry::TelemetryStats Telemetry::getStats() TelemetryStats stats; stats.txBytes = utalkStats.txBytes; - stats.rxBytes = utalkStats.rxBytes; stats.txObjectBytes = utalkStats.txObjectBytes; - stats.rxObjectBytes = utalkStats.rxObjectBytes; - stats.rxObjects = utalkStats.rxObjects; stats.txObjects = utalkStats.txObjects; stats.txErrors = utalkStats.txErrors + txErrors; - stats.rxErrors = utalkStats.rxErrors; stats.txRetries = txRetries; + stats.rxBytes = utalkStats.rxBytes; + stats.rxObjectBytes = utalkStats.rxObjectBytes; + stats.rxObjects = utalkStats.rxObjects; + stats.rxErrors = utalkStats.rxErrors; + stats.rxSyncErrors = utalkStats.rxSyncErrors; + stats.rxCrcErrors = utalkStats.rxCrcErrors; + // Done return stats; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 327a9d5d0..c4ff2374f 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h @@ -60,14 +60,17 @@ class Telemetry : public QObject { public: typedef struct { quint32 txBytes; - quint32 rxBytes; quint32 txObjectBytes; - quint32 rxObjectBytes; - quint32 rxObjects; quint32 txObjects; quint32 txErrors; - quint32 rxErrors; quint32 txRetries; + + quint32 rxBytes; + quint32 rxObjectBytes; + quint32 rxObjects; + quint32 rxErrors; + quint32 rxSyncErrors; + quint32 rxCrcErrors; } TelemetryStats; Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp index 8ce52e2bd..f0068c6cf 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp @@ -199,12 +199,17 @@ void TelemetryMonitor::processStatsUpdates() tel->resetStats(); // Update stats object - gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); - gcsStats.RxFailures += telStats.rxErrors; + gcsStats.TxBytes += telStats.txBytes; gcsStats.TxFailures += telStats.txErrors; gcsStats.TxRetries += telStats.txRetries; + gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.RxBytes += telStats.rxBytes; + gcsStats.RxFailures += telStats.rxErrors; + gcsStats.RxSyncErrors += telStats.rxSyncErrors; + gcsStats.RxCrcErrors += telStats.rxCrcErrors; + // Check for a connection timeout bool connectionTimeout; if (telStats.rxObjects > 0) { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index e8a67f53d..3a2acb459 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -33,7 +33,7 @@ #include #ifdef VERBOSE_UAVTALK -#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC || objId == 0x2472A0AE) +#define VERBOSE_FILTER(objId) if (objId == 0x173E3850 || objId == 0x99C63292) #endif #define SYNC_VAL 0x3C @@ -255,7 +255,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxbyte != SYNC_VAL) { // continue until sync byte is matched - // TODO stats.rxSyncErrror++; + stats.rxSyncErrors++; break; } @@ -282,7 +282,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if ((rxbyte & TYPE_MASK) != TYPE_VER) { qWarning() << "UAVTalk - error : bad type"; - // TODO stats.rxSyncErrror++; + stats.rxErrors++; rxState = STATE_SYNC; break; } @@ -421,7 +421,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCS != rxCSPacket) { // packet error - faulty CRC qWarning() << "UAVTalk - error : failed CRC check" << rxObjId; - stats.rxErrors++; + stats.rxCrcErrors++; rxState = STATE_SYNC; break; } @@ -453,8 +453,8 @@ bool UAVTalk::processInputByte(quint8 rxbyte) default: qWarning() << "UAVTalk - error : bad state"; - stats.rxErrors++; rxState = STATE_SYNC; + break; } // Done @@ -518,7 +518,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * #endif if (obj != NULL) { // Object updated or created, transmit ACK - transmitObject(TYPE_ACK, objId, instId, obj); + error = !transmitObject(TYPE_ACK, objId, instId, obj); } else { error = true; } @@ -545,7 +545,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 * if (obj != NULL) { // Object found, transmit it // The sent object will ack the object request on the receiver side - transmitObject(TYPE_OBJ, objId, instId, obj); + error = !transmitObject(TYPE_OBJ, objId, instId, obj); } else { error = true; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 85a769a1b..87648a4f5 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -48,13 +48,16 @@ public: typedef struct { quint32 txBytes; - quint32 rxBytes; quint32 txObjectBytes; - quint32 rxObjectBytes; - quint32 rxObjects; quint32 txObjects; quint32 txErrors; + + quint32 rxBytes; + quint32 rxObjectBytes; + quint32 rxObjects; quint32 rxErrors; + quint32 rxSyncErrors; + quint32 rxCrcErrors; } ComStats; UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr); @@ -105,28 +108,41 @@ private: static const quint8 crc_table[256]; // Types - typedef enum { STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS } RxStateType; + typedef enum { + STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS + } RxStateType; // Variables QPointer io; + UAVObjectManager *objMngr; + + ComStats stats; + QMutex mutex; + QMap *> transMap; + +// quint16 rxReadOffset; +// quint16 rxSyncOffset; quint8 rxBuffer[MAX_PACKET_LENGTH]; + quint8 txBuffer[MAX_PACKET_LENGTH]; + // Variables used by the receive state machine + // state machine variables + qint32 rxCount; + qint32 packetSize; + RxStateType rxState; + // data variables quint8 rxTmpBuffer[4]; quint8 rxType; quint32 rxObjId; quint16 rxInstId; quint16 rxLength; quint16 rxPacketLength; - - quint8 rxCSPacket, rxCS; - qint32 rxCount; - qint32 packetSize; - RxStateType rxState; - ComStats stats; + quint8 rxCSPacket; + quint8 rxCS; bool useUDPMirror; QUdpSocket *udpSocketTx; diff --git a/shared/uavobjectdefinition/gcstelemetrystats.xml b/shared/uavobjectdefinition/gcstelemetrystats.xml index 9cdf9985d..540861c11 100644 --- a/shared/uavobjectdefinition/gcstelemetrystats.xml +++ b/shared/uavobjectdefinition/gcstelemetrystats.xml @@ -1,12 +1,18 @@ The telemetry statistics from the ground computer + - + - + + + + + + From 36788a60f42a9bd0611d4006f8f41140ddc98164 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 16:26:59 +0100 Subject: [PATCH 072/113] bugfix for xplane hitl, convert gyroscope sensor from rad/s to deg/s --- ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index 53609951e..20beab45d 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -318,10 +318,10 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.velEast = velX; out.velDown = -velZ; - // Update gyroscope sensor data - out.rollRate = rollRate_rad; - out.pitchRate = pitchRate_rad; - out.yawRate = yawRate_rad; + // Update gyroscope sensor data - convert from rad/s to deg/s + out.rollRate = rollRate_rad * (180.0/M_PI); + out.pitchRate = pitchRate_rad * (180.0/M_PI); + out.yawRate = yawRate_rad * (180.0/M_PI); // Update accelerometer sensor data out.accX = accX; From 1cccd152bd7adb68c95d1467ceb6efc3ccc42591 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 16:50:30 +0100 Subject: [PATCH 073/113] added AltitudeHold to simposix --- flight/targets/boards/simposix/firmware/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index c9d719334..c64974d0a 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += FirmwareIAP MODULES += StateEstimation #MODULES += Sensors/simulated/Sensors MODULES += Airspeed +MODULES += AltitudeHold #MODULES += OveroSync # Paths From d60eda1a83449accfc956ab835bc95060d5d1109 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 18:37:14 +0100 Subject: [PATCH 074/113] forgot uncrustification --- ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index 20beab45d..e7b409815 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -319,9 +319,9 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.velDown = -velZ; // Update gyroscope sensor data - convert from rad/s to deg/s - out.rollRate = rollRate_rad * (180.0/M_PI); - out.pitchRate = pitchRate_rad * (180.0/M_PI); - out.yawRate = yawRate_rad * (180.0/M_PI); + out.rollRate = rollRate_rad * (180.0 / M_PI); + out.pitchRate = pitchRate_rad * (180.0 / M_PI); + out.yawRate = yawRate_rad * (180.0 / M_PI); // Update accelerometer sensor data out.accX = accX; From 505d334c4beee15f837e27c1b0ba216543771f3f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 18:37:27 +0100 Subject: [PATCH 075/113] simplified altitude hold control loop --- flight/modules/AltitudeHold/altitudehold.c | 54 ++++--------------- .../altitudeholdsettings.xml | 6 +-- .../altitudeholdstatus.xml | 2 - 3 files changed, 11 insertions(+), 51 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 153ffcf24..e3e5fcd2a 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -70,14 +70,12 @@ // Private variables static DelayedCallbackInfo *altitudeHoldCBInfo; static AltitudeHoldSettingsData altitudeHoldSettings; -static struct pid accelpid; -static float accelStateDown; +static struct pid pid0, pid1; // Private functions static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); -static void AccelStateUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -106,7 +104,6 @@ int32_t AltitudeHoldInitialize() altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); - AccelStateConnectCallback(&AccelStateUpdatedCb); return 0; } @@ -129,7 +126,8 @@ static void altitudeHoldTask(void) case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: break; default: - pid_zero(&accelpid); + pid_zero(&pid0); + pid_zero(&pid1); StabilizationDesiredThrottleGet(&startThrottle); DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; @@ -149,40 +147,13 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (altitudeHoldDesired.Altitude - positionStateDown) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); // velocity control loop - altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (altitudeHoldStatus.VelocityDesired - velocityStateDown) - 9.81f; - - altitudeHoldStatus.AccelerationFiltered = accelStateDown; + float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); AltitudeHoldStatusSet(&altitudeHoldStatus); - // compensate acceleration by rotation - // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q - // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation - // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical - // acceleration at the current tilt angle. - // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep - // integrals from winding in any direction - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rbe[3][3]; - Quaternion2R(&attitudeState.q1, Rbe); - - float rotatedAccelDesired = altitudeHoldStatus.AccelerationDesired; -#if 0 - if (fabsf(Rbe[2][2]) > 1e-3f) { - rotatedAccelDesired /= Rbe[2][2]; - } else { - rotatedAccelDesired = accelStateDown; - } -#endif - - // acceleration control loop - float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); - if (throttle >= 1.0f) { throttle = 1.0f; } @@ -204,18 +175,11 @@ static void altitudeHoldTask(void) DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); } -static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - float down; - - AccelStatezGet(&down); - accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); -} - static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); - pid_zero(&accelpid); - accelStateDown = 0.0f; + pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit); + pid_zero(&pid0); + pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit); + pid_zero(&pid1); } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 26491eb2f..f956cca30 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,10 +1,8 @@ Settings for the @ref AltitudeHold module - - - - + + diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml index 928496f51..f298a45ab 100644 --- a/shared/uavobjectdefinition/altitudeholdstatus.xml +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -2,8 +2,6 @@ Status Data from Altitude Hold Control Loops - - From cefcb9881ad7147b9653eb8dfbb1472942af81d2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 29 Dec 2013 19:09:54 +0100 Subject: [PATCH 076/113] Altitude Hold - compensation for tilt --- flight/modules/AltitudeHold/altitudehold.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e3e5fcd2a..f177d9c8b 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,25 @@ static void altitudeHoldTask(void) // altitude control loop altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + AltitudeHoldStatusSet(&altitudeHoldStatus); + // velocity control loop float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); - AltitudeHoldStatusSet(&altitudeHoldStatus); + // compensate throttle for current attitude + // Rbe[2][2] in the rotation matrix is the rotated down component of a + // 0,0,1 vector + // it is used to scale the throttle, but only up to 4 times the regular + // throttle + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1, Rbe); + if (fabsf(Rbe[2][2]) > 0.25f) { + throttle /= Rbe[2][2]; + } else { + throttle /= 0.25f; + } if (throttle >= 1.0f) { throttle = 1.0f; From b786cec832d7fb5a8b7ecc57ae99ae996d8ba8c4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 30 Dec 2013 01:40:27 +0100 Subject: [PATCH 077/113] bugfix to eventdispatcher to fix uavtalk issue on coptercontrol --- flight/uavobjects/eventdispatcher.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 9fc78a21c..d9518d620 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -104,6 +104,7 @@ int32_t EventDispatcherInitialize() // Create callback eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4); + DelayedCallbackDispatch(eventSchedulerCallback); // Done return 0; From a322c14d35d17113aa0c42eca1da5dd1766291e8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 2 Jan 2014 17:17:19 +0100 Subject: [PATCH 078/113] changed defaults for altitude hold control loop coefficients --- shared/uavobjectdefinition/altitudeholdsettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index f956cca30..61724dc35 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,8 +1,8 @@ Settings for the @ref AltitudeHold module - - + + From cceb9a363cb00488bf76502e0af02b4214ad8dae Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 5 Jan 2014 18:45:57 +0100 Subject: [PATCH 079/113] [OP-1105] improved handling of missing firmware description in GCS --- .../plugins/config/configpipxtremewidget.cpp | 4 +- .../uavobjectutil/uavobjectutilmanager.cpp | 9 ++++- .../uavobjectutil/uavobjectutilmanager.h | 1 + .../src/plugins/uploader/devicewidget.cpp | 37 ++++++++----------- .../src/plugins/uploader/op_dfu.cpp | 6 ++- .../plugins/uploader/runningdevicewidget.cpp | 24 ++++++------ 6 files changed, 41 insertions(+), 40 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index b9035e7fe..d8a495a82 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -168,8 +168,9 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) } // Update the Description field + // TODO use UAVObjectUtilManager::descriptionToStructure() UAVObjectField *descField = object->getField("Description"); - if (descField) { + if (descField && descField->getValue(0) != QChar(255)) { /* * This looks like a binary with a description at the end: * 4 bytes: header: "OpFw". @@ -196,6 +197,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) m_oplink->FirmwareVersion->setText(descstr + " " + date); } else { qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + m_oplink->FirmwareVersion->setText(tr("Unknown")); } // Update the serial number field diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index ebcdcecda..2edd61186 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -299,6 +299,13 @@ QByteArray UAVObjectUtilManager::getBoardDescription() return ret; } +QString UAVObjectUtilManager::getBoardDescriptionString() +{ + QByteArray arr = getBoardDescription(); + + int index = arr.indexOf(255); + return QString((index == -1) ? arr : arr.left(index)); +} // ****************************** // HomeLocation @@ -472,5 +479,3 @@ bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescrip } return false; } - -// ****************************** diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 7ad27b8d1..a89e241f7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -61,6 +61,7 @@ public: QByteArray getBoardCPUSerial(); quint32 getFirmwareCRC(); QByteArray getBoardDescription(); + QString getBoardDescriptionString(); deviceDescriptorStruct getBoardDescriptionStruct(); static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc); UAVObjectManager *getObjectManager(); diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index 0cb91125a..9afbe5bdb 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -41,8 +41,7 @@ DeviceWidget::DeviceWidget(QWidget *parent) : connect(myDevice->updateButton, SIGNAL(clicked()), this, SLOT(uploadFirmware())); connect(myDevice->pbLoad, SIGNAL(clicked()), this, SLOT(loadFirmware())); connect(myDevice->confirmCheckBox, SIGNAL(stateChanged(int)), this, SLOT(confirmCB(int))); - QPixmap pix = QPixmap(QString(":uploader/images/view-refresh.svg")); - myDevice->statusIcon->setPixmap(pix); + myDevice->statusIcon->setPixmap(QPixmap(":uploader/images/view-refresh.svg")); myDevice->lblCertified->setText(""); } @@ -79,12 +78,12 @@ void DeviceWidget::populate() { int id = m_dfu->devices[deviceID].ID; - myDevice->lbldevID->setText(QString("Device ID: ") + QString::number(id, 16)); + myDevice->lbldevID->setText(tr("Device ID: ") + QString::number(id, 16)); // DeviceID tells us what sort of HW we have detected: // display a nice icon: myDevice->gVDevice->scene()->clear(); myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id)); - myDevice->lblHWRev->setText(QString(tr("HW Revision: ")) + QString::number(id & 0x00FF, 16)); + myDevice->lblHWRev->setText(tr("HW Revision: ") + QString::number(id & 0x00FF, 16)); switch (id) { case 0x0101: @@ -115,25 +114,23 @@ void DeviceWidget::populate() bool r = m_dfu->devices[deviceID].Readable; bool w = m_dfu->devices[deviceID].Writable; - myDevice->lblAccess->setText(QString("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); - myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); + myDevice->lblAccess->setText(tr("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); + myDevice->lblMaxCode->setText(tr("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC)); - myDevice->lblBLVer->setText(QString("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); + myDevice->lblBLVer->setText(tr("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; m_dfu->enterDFU(deviceID); QByteArray desc = m_dfu->DownloadDescriptionAsBA(size); if (!populateBoardStructuredDescription(desc)) { - // TODO // desc was not a structured description QString str = m_dfu->DownloadDescription(size); - myDevice->lblDescription->setText(QString("Firmware custom description: ") + str.left(str.indexOf(QChar(255)))); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblDescription->setText((!str.isEmpty()) ? str : tr("Unknown")); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); - myDevice->lblBuildDate->setText("Warning: development firmware"); - myDevice->lblGitTag->setText(""); - myDevice->lblBrdName->setText(""); + myDevice->lblBuildDate->setText(tr("Unknown")); + myDevice->lblGitTag->setText(tr("Unknown")); + myDevice->lblBrdName->setText(tr("Unknown")); } status("Ready...", STATUSICON_INFO); @@ -179,13 +176,11 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc) myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-")); if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescription->setText(onBoardDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); } else { myDevice->lblDescription->setText(onBoardDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } @@ -205,14 +200,12 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc) if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); - myDevice->lblCertifiedL->setPixmap(pix); + myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/application-certificate.svg")); myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build")); } else { myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertifiedL->setPixmap(pix); + myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision)); diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 2ebb1495e..c3dcd20ad 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -342,8 +342,9 @@ QString DFUObject::DownloadDescription(int const & numberOfChars) QByteArray arr; StartDownloadT(&arr, numberOfChars, OP_DFU::Descript); - QString str(arr); - return str; + + int index = arr.indexOf(255); + return QString((index == -1) ? arr : arr.left(index)); } QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars) @@ -351,6 +352,7 @@ QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars) QByteArray arr; StartDownloadT(&arr, numberOfChars, OP_DFU::Descript); + return arr; } diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index 904080ef5..b433f9641 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -104,24 +104,22 @@ void RunningDeviceWidget::populate() deviceDescriptorStruct devDesc; if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { - myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); } else { - myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } - myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash); - myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); + myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + devDesc.gitHash); + myDevice->lblFWDate->setText(tr("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); } else { - myDevice->lblFWTag->setText(QString("Firmware tag: ") + QString(description).left(QString(description).indexOf(QChar(255)))); - myDevice->lblGitCommitTag->setText("Git commit tag: Unknown"); - myDevice->lblFWDate->setText(QString("Firmware date: Unknown")); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + QString desc = utilMngr->getBoardDescriptionString(); + myDevice->lblFWTag->setText(tr("Firmware tag: ") + (!desc.isEmpty() ? desc : tr("Unknown"))); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); + myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + tr("Unknown")); + myDevice->lblFWDate->setText(tr("Firmware date: ") + tr("Unknown")); } } From ec421ad9621c577710c26a12ee74df195d902909 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 5 Jan 2014 22:27:54 +0100 Subject: [PATCH 080/113] [OP-1105] improved handling of missing firmware description in GCS - fixed logging spam introduced by previous commit --- .../plugins/config/configpipxtremewidget.cpp | 66 ++++++++++--------- 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index d8a495a82..c97c60899 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -151,7 +151,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) m_oplink->PairID4->setEnabled(false); m_oplink->Bind4->setEnabled(pairid4); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + qDebug() << "ConfigPipXtremeWidget: Count not read PairID field."; } UAVObjectField *pairRssiField = object->getField("PairSignalStrengths"); if (pairRssiField) { @@ -164,40 +164,44 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + qDebug() << "ConfigPipXtremeWidget: Count not read PairID field."; } // Update the Description field // TODO use UAVObjectUtilManager::descriptionToStructure() UAVObjectField *descField = object->getField("Description"); - if (descField && descField->getValue(0) != QChar(255)) { - /* - * This looks like a binary with a description at the end: - * 4 bytes: header: "OpFw". - * 4 bytes: GIT commit tag (short version of SHA1). - * 4 bytes: Unix timestamp of compile time. - * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. - * 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded. - * 20 bytes: SHA1 sum of the firmware. - * 20 bytes: SHA1 sum of the uavo definitions. - * 20 bytes: free for now. - */ - char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; - for (unsigned int i = 0; i < 26; ++i) { - buf[i] = descField->getValue(i + 14).toChar().toLatin1(); + if (descField) { + if (descField->getValue(0) != QChar(255)) { + /* + * This looks like a binary with a description at the end: + * 4 bytes: header: "OpFw". + * 4 bytes: GIT commit tag (short version of SHA1). + * 4 bytes: Unix timestamp of compile time. + * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. + * 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded. + * 20 bytes: SHA1 sum of the firmware. + * 20 bytes: SHA1 sum of the uavo definitions. + * 20 bytes: free for now. + */ + char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; + for (unsigned int i = 0; i < 26; ++i) { + buf[i] = descField->getValue(i + 14).toChar().toLatin1(); + } + buf[26] = '\0'; + QString descstr(buf); + quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF; + for (int i = 1; i < 4; i++) { + gitDate = gitDate << 8; + gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF; + } + QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); + m_oplink->FirmwareVersion->setText(descstr + " " + date); + } else { + m_oplink->FirmwareVersion->setText(tr("Unknown")); } - buf[26] = '\0'; - QString descstr(buf); - quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF; - for (int i = 1; i < 4; i++) { - gitDate = gitDate << 8; - gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF; - } - QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); - m_oplink->FirmwareVersion->setText(descstr + " " + date); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; - m_oplink->FirmwareVersion->setText(tr("Unknown")); + } + else { + qDebug() << "ConfigPipXtremeWidget: Failed to read Description field."; } // Update the serial number field @@ -213,7 +217,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; m_oplink->SerialNumber->setText(buf); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + qDebug() << "ConfigPipXtremeWidget: Failed to read CPUSerial field."; } // Update the link state @@ -221,7 +225,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) if (linkField) { m_oplink->LinkState->setText(linkField->getValue().toString()); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; + qDebug() << "ConfigPipXtremeWidget: Failed to read LinkState field."; } } From 772c395e67994bbefd462905a9392b15ad1fa4b1 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 6 Jan 2014 21:35:00 +0100 Subject: [PATCH 081/113] OP-1122 OP-1145 fixed handling of ground messages done by OPLM - OPLM used to receive and relay all messages - OPLM will now reveice only specific messages and relay others as required (needs to be reviewed...) - add RadioComBridgeStats uavobject to collect and report tx/rx statistics - made error handling more robust - added a few FIXMEs --- .../modules/RadioComBridge/RadioComBridge.c | 202 ++++++++++++++---- .../boards/oplinkmini/firmware/Makefile | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../radiocombridgestats.xml | 26 +++ 4 files changed, 193 insertions(+), 38 deletions(-) create mode 100644 shared/uavobjectdefinition/radiocombridgestats.xml diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 88b2bb9d9..f913ab672 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,7 @@ #define SERIAL_RX_BUF_LEN 100 #define PPM_INPUT_TIMEOUT 100 + // **************** // Private types @@ -81,10 +83,11 @@ typedef struct { uint8_t serialRxBuf[SERIAL_RX_BUF_LEN]; // Error statistics. - uint32_t comTxErrors; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - uint32_t droppedPackets; + uint32_t telemetryTxRetries; + uint32_t radioTxRetries; + + // Is this modem the coordinator + bool isCoordinator; // Should we parse UAVTalk? bool parseUAVTalk; @@ -107,6 +110,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); +static void registerObject(UAVObjHandle obj); // **************** // Private variables @@ -124,7 +128,9 @@ static int32_t RadioComBridgeStart(void) // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + + // Check if this is the coordinator modem + data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && @@ -165,12 +171,16 @@ static int32_t RadioComBridgeStart(void) // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); - if (is_coordinator) { + if (data->isCoordinator) { UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); } else { UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); } + if (data->isCoordinator) { + registerObject(RadioComBridgeStatsHandle()); + } + // Configure the UAVObject callbacks ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); @@ -223,6 +233,7 @@ static int32_t RadioComBridgeInitialize(void) OPLinkStatusInitialize(); ObjectPersistenceInitialize(); OPLinkReceiverInitialize(); + RadioComBridgeStatsInitialize(); // Initialise UAVTalk data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); @@ -233,9 +244,9 @@ static int32_t RadioComBridgeInitialize(void) data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); // Initialize the statistics. - data->comTxErrors = 0; - data->comTxRetries = 0; - data->UAVTalkErrors = 0; + data->telemetryTxRetries = 0; + data->radioTxRetries = 0; + data->parseUAVTalk = true; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; PIOS_COM_RADIO = PIOS_COM_RFM22B; @@ -244,6 +255,71 @@ static int32_t RadioComBridgeInitialize(void) } MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart); + +// TODO this code (badly) duplicates code from telemetry.c +// This method should be used only for periodically updated objects. +// The register method defined in telemetry.c should be factored out in a shared location so it can be +// used from here... +static void registerObject(UAVObjHandle obj) +{ + // Setup object for periodic updates + UAVObjEvent ev = { + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .event = EV_UPDATED_PERIODIC, + }; + + // Get metadata + UAVObjMetadata metadata; + UAVObjGetMetadata(obj, &metadata); + + EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod); + UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ); +} + +/** + * Update telemetry statistics + */ +static void updateRadioComBridgeStats() +{ + UAVTalkStats telemetryUAVTalkStats; + UAVTalkStats radioUAVTalkStats; + RadioComBridgeStatsData radioComBridgeStats; + + // Get telemetry stats + UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats); + UAVTalkResetStats(data->telemUAVTalkCon); + + // Get radio stats + UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats); + UAVTalkResetStats(data->radioUAVTalkCon); + + // Get stats object data + RadioComBridgeStatsGet(&radioComBridgeStats); + + // Update stats object + radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; + radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; + radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; + + radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; + radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; + radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors; + radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; + + radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; + radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; + radioComBridgeStats.RadioTxRetries += data->radioTxRetries; + + radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; + radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; + radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors; + radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; + + // Update stats object data + RadioComBridgeStatsSet(&radioComBridgeStats); +} + /** * @brief Telemetry transmit task, regular priority * @@ -260,18 +336,20 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) #endif // Wait for queue message if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { - if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { - // Send update (with retries) - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) { - ++retries; - } - } - data->comTxRetries += retries; + if (ev.obj == RadioComBridgeStatsHandle()) { + updateRadioComBridgeStats(); } + // Send update (with retries) + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0; + if (success == -1) { + ++retries; + } + } + // Update stats + data->telemetryTxRetries += retries; } } } @@ -299,12 +377,12 @@ static void radioTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) { + success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0; + if (success == -1) { ++retries; } } - data->comTxRetries += retries; + data->radioTxRetries += retries; } } } @@ -333,6 +411,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters) } } else if (PIOS_COM_TELEMETRY) { // Send the data straight to the telemetry port. + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); } } @@ -425,8 +505,10 @@ static void serialRxTask(__attribute__((unused)) void *parameters) // Receive some data. uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY); - // Send the data over the radio link. if (bytes_to_process > 0) { + // Send the data over the radio link. + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process); } } else { @@ -445,6 +527,7 @@ static void serialRxTask(__attribute__((unused)) void *parameters) */ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { + int32_t ret; uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; #if defined(PIOS_INCLUDE_USB) @@ -454,10 +537,13 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) } #endif /* PIOS_INCLUDE_USB */ if (outputPort) { - return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... + ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { - return -1; + ret = -1; } + return ret; } /** @@ -477,10 +563,11 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) // Don't send any data unless the radio port is available. if (outputPort && PIOS_COM_Available(outputPort)) { + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { - // For some reason, if this function returns failure, it prevents saving settings. - return length; + return -1; } } @@ -494,12 +581,40 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) { // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); - if (state == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - } else if (state == UAVTALK_STATE_COMPLETE) { - UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain telemetry objects + uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); + switch (objId) { + case OPLINKSTATUS_OBJID: + case OPLINKSETTINGS_OBJID: + case OPLINKRECEIVER_OBJID: + case MetaObjectId(OPLINKSTATUS_OBJID): + case MetaObjectId(OPLINKSETTINGS_OBJID): + case MetaObjectId(OPLINKRECEIVER_OBJID): + UAVTalkReceiveObject(inConnectionHandle); + break; + case OBJECTPERSISTENCE_OBJID: + case MetaObjectId(OBJECTPERSISTENCE_OBJID): + // receive object locally + // some objects will send back a response to telemetry + // FIXME: + // OPLM will ack or nack all objects requests and acked object sends + // Receiver will probably also ack / nack the same messages + // This has some consequences like : + // Second ack/nack will not match an open transaction or will apply to wrong transaction + // Question : how does GCS handle receiving the same object twice + // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks... + UAVTalkReceiveObject(inConnectionHandle); + // relay packet to remote modem + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + default: + // all other packets are relayed to the remote modem + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + } } } @@ -515,19 +630,30 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn // Keep reading until we receive a completed packet. UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); - if (state == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - } else if (state == UAVTALK_STATE_COMPLETE) { - // We only want to unpack certain objects from the remote modem. + if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain objects from the remote modem + // Similarly we only want to relay certain objects to the telemetry port uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); switch (objId) { case OPLINKSTATUS_OBJID: case OPLINKSETTINGS_OBJID: + case MetaObjectId(OPLINKSTATUS_OBJID): + case MetaObjectId(OPLINKSETTINGS_OBJID): + // Ignore object... + // These objects are shadowed by the modem and are not transmitted to the telemetry port + // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead + // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead break; case OPLINKRECEIVER_OBJID: + case MetaObjectId(OPLINKRECEIVER_OBJID): + // Receive object locally + // These objects are received by the modem and are not transmitted to the telemetry port + // - OPLINKRECEIVER_OBJID : not sure why + // some objects will send back a response to the remote modem UAVTalkReceiveObject(inConnectionHandle); break; default: + // all other packets are relayed to the telemetry port UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); break; } @@ -545,7 +671,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE ObjectPersistenceGet(&obj_per); - // Is this concerning or setting object? + // Is this concerning our setting object? if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { // Is this a save, load, or delete? bool success = false; diff --git a/flight/targets/boards/oplinkmini/firmware/Makefile b/flight/targets/boards/oplinkmini/firmware/Makefile index 12a491df5..3a9c6b113 100644 --- a/flight/targets/boards/oplinkmini/firmware/Makefile +++ b/flight/targets/boards/oplinkmini/firmware/Makefile @@ -53,6 +53,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/oplinksettings.c SRC += $(OPUAVSYNTHDIR)/objectpersistence.c SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c + SRC += $(OPUAVSYNTHDIR)/radiocombridgestats.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d1b002994..f65bffd4d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -111,6 +111,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/oplinksettings.h \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \ $$UAVOBJECT_SYNTHETICS/oplinkreceiver.h \ + $$UAVOBJECT_SYNTHETICS/radiocombridgestats.h \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ $$UAVOBJECT_SYNTHETICS/waypoint.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ @@ -203,6 +204,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkreceiver.cpp \ + $$UAVOBJECT_SYNTHETICS/radiocombridgestats.cpp \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/waypoint.cpp \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ diff --git a/shared/uavobjectdefinition/radiocombridgestats.xml b/shared/uavobjectdefinition/radiocombridgestats.xml new file mode 100644 index 000000000..1fd357924 --- /dev/null +++ b/shared/uavobjectdefinition/radiocombridgestats.xml @@ -0,0 +1,26 @@ + + + Maintains the telemetry statistics from the OPLM RadioComBridge. + + + + + + + + + + + + + + + + + + + + + + + From fe4fe46e346a194f657e9fdb3b80f9f27753acce Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 6 Jan 2014 21:45:01 +0100 Subject: [PATCH 082/113] OP-1122 OP-1145 flight uavtalk minor typo fix and minor error handling improvment --- flight/uavtalk/uavtalk.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 7e49344de..001fac196 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -34,7 +34,7 @@ //#define UAV_DEBUGLOG 1 -#if defined UAV_DEBUGLOG && defined(FLASH_FREETOS) +#if defined UAV_DEBUGLOG && defined FLASH_FREERTOS #define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__) #define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == 0x5E5903CC) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } #else @@ -678,9 +678,7 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) return -1; } - receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer); - - return 0; + return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer); } /** From 03b3dca76cf98e1a08042328d43eb88d463e2a65 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 6 Jan 2014 21:48:34 +0100 Subject: [PATCH 083/113] OP-1122 OP-1145 flight telemetry improvement - create periodic queue events only for periodically updated uavobjects - made error handling more robust - added a FIXME concerning FlighTelemetryStats as it is updated peridocally using two separate mechanisms --- flight/modules/Telemetry/telemetry.c | 49 ++++++++++++++++++---------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 78a498340..443112773 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -158,6 +158,7 @@ int32_t TelemetryInitialize(void) #endif // Create periodic event that will be used to update the telemetry stats + // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms... txErrors = 0; txRetries = 0; UAVObjEvent ev; @@ -177,18 +178,18 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart); static void registerObject(UAVObjHandle obj) { if (UAVObjIsMetaobject(obj)) { - /* Only connect change notifications for meta objects. No periodic updates */ + // Only connect change notifications for meta objects. No periodic updates UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); } else { // Setup object for periodic updates - UAVObjEvent ev = { - .obj = obj, - .instId = UAVOBJ_ALL_INSTANCES, - .event = EV_UPDATED_PERIODIC, - }; - EventPeriodicQueueCreate(&ev, queue, 0); - ev.event = EV_LOGGING_PERIODIC; - EventPeriodicQueueCreate(&ev, queue, 0); +// UAVObjEvent ev = { +// .obj = obj, +// .instId = UAVOBJ_ALL_INSTANCES, +// .event = EV_UPDATED_PERIODIC, +// }; +// EventPeriodicQueueCreate(&ev, queue, 0); +// ev.event = EV_LOGGING_PERIODIC; +// EventPeriodicQueueCreate(&ev, queue, 0); // Setup object for telemetry updates updateObject(obj, EV_NONE); @@ -295,7 +296,6 @@ static void processObjEvent(UAVObjEvent *ev) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode; - FlightTelemetryStatsData flightStats; int32_t retries; int32_t success; @@ -304,7 +304,6 @@ static void processObjEvent(UAVObjEvent *ev) } else if (ev->obj == GCSTelemetryStatsHandle()) { gcsTelemetryStatsUpdated(); } else { - FlightTelemetryStatsGet(&flightStats); // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); updateMode = UAVObjGetTelemetryUpdateMode(&metadata); @@ -312,7 +311,9 @@ static void processObjEvent(UAVObjEvent *ev) // Act on event retries = 0; success = -1; - if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { + if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) + || ev->event == EV_UPDATED_MANUAL + || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until ack is received or timeout @@ -355,7 +356,9 @@ static void processObjEvent(UAVObjEvent *ev) // Log UAVObject if necessary if (ev->obj) { updateMode = UAVObjGetLoggingUpdateMode(&metadata); - if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_LOGGING_MANUAL || ((ev->event == EV_LOGGING_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { + if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) + || ev->event == EV_LOGGING_MANUAL + || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { if (ev->instId == UAVOBJ_ALL_INSTANCES) { success = UAVObjGetNumInstances(ev->obj); for (retries = 0; retries < success; retries++) { @@ -488,12 +491,18 @@ static int32_t transmitData(uint8_t *data, int32_t length) static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) { UAVObjEvent ev; + int32_t ret; - // Add object for periodic updates + // Add or update object for periodic updates ev.obj = obj; ev.instId = UAVOBJ_ALL_INSTANCES; ev.event = EV_UPDATED_PERIODIC; - return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); + + ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); + if (ret == -1) { + ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs); + } + return ret; } /** @@ -506,12 +515,18 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs) { UAVObjEvent ev; + int32_t ret; - // Add object for periodic updates + // Add or update object for periodic updates ev.obj = obj; ev.instId = UAVOBJ_ALL_INSTANCES; ev.event = EV_LOGGING_PERIODIC; - return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); + + ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); + if (ret == -1) { + ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs); + } + return ret; } /** From ee1584d6396bdcbe1693622f54b911d307d866ad Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 7 Jan 2014 21:55:26 +0100 Subject: [PATCH 084/113] OP-1122 OP-1145 uavobject toStringBrief now displays the object Id as in hexadecimal format --- ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 68295453f..a43f893ef 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -473,9 +473,10 @@ QString UAVObject::toStringBrief() { QString sout; + // object Id is converted to uppercase hexadecimal sout.append(QString("%1 (ID: %2-%3, %4 bytes, %5)") .arg(getName()) - .arg(getObjID()) + .arg(getObjID(), 1, 16).toUpper() .arg(getInstID()) .arg(getNumBytes()) .arg(isSingleInstance() ? "single" : "multiple")); From 339e69f25dbb233b30db01824e21f08538c1b521 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 00:00:00 +0100 Subject: [PATCH 085/113] OP-1122 OP-1123 fixed OSX specific GCS compilation errors --- .../src/plugins/opmap/flightdatamodel.cpp | 326 +++++++----------- 1 file changed, 134 insertions(+), 192 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index 27e085e93..ad269d626 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -28,8 +28,10 @@ #include "flightdatamodel.h" #include #include + flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent) -{} +{ +} int flightDataModel::rowCount(const QModelIndex & /*parent*/) const { @@ -72,332 +74,270 @@ QVariant flightDataModel::data(const QModelIndex &index, int role) const bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value) { + bool b; switch (index) { case WPDESCRITPTION: row->wpDescritption = value.toString(); - return true; - + b = true; break; case LATPOSITION: row->latPosition = value.toDouble(); - return true; - + b = true; break; case LNGPOSITION: row->lngPosition = value.toDouble(); - return true; - + b = true; break; case DISRELATIVE: row->disRelative = value.toDouble(); - return true; - + b = true; break; case BEARELATIVE: row->beaRelative = value.toDouble(); - return true; - + b = true; break; case ALTITUDERELATIVE: row->altitudeRelative = value.toFloat(); - return true; - + b = true; break; case ISRELATIVE: row->isRelative = value.toBool(); - return true; - + b = true; break; case ALTITUDE: row->altitude = value.toDouble(); - return true; - + b = true; break; case VELOCITY: row->velocity = value.toFloat(); - return true; - + b = true; break; case MODE: row->mode = value.toInt(); - return true; - + b = true; break; case MODE_PARAMS0: row->mode_params[0] = value.toFloat(); - return true; - + b = true; break; case MODE_PARAMS1: row->mode_params[1] = value.toFloat(); - return true; - + b = true; break; case MODE_PARAMS2: row->mode_params[2] = value.toFloat(); - return true; - + b = true; break; case MODE_PARAMS3: row->mode_params[3] = value.toFloat(); - return true; - + b = true; break; case CONDITION: row->condition = value.toInt(); - return true; - + b = true; break; case CONDITION_PARAMS0: row->condition_params[0] = value.toFloat(); - return true; - + b = true; break; case CONDITION_PARAMS1: row->condition_params[1] = value.toFloat(); - return true; - + b = true; break; case CONDITION_PARAMS2: row->condition_params[2] = value.toFloat(); - return true; - + b = true; break; case CONDITION_PARAMS3: row->condition_params[3] = value.toFloat(); - return true; - + b = true; break; case COMMAND: row->command = value.toInt(); + b = true; break; case JUMPDESTINATION: row->jumpdestination = value.toInt(); - return true; - + b = true; break; case ERRORDESTINATION: row->errordestination = value.toInt(); - return true; - + b = true; break; case LOCKED: row->locked = value.toBool(); - return true; - + b = true; break; default: - return false; + b = false; + break; } - return false; + return b; } QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const { + QVariant value; switch (index) { case WPDESCRITPTION: - return row->wpDescritption; - + value = row->wpDescritption; break; case LATPOSITION: - return row->latPosition; - + value = row->latPosition; break; case LNGPOSITION: - return row->lngPosition; - + value = row->lngPosition; break; case DISRELATIVE: - return row->disRelative; - + value = row->disRelative; break; case BEARELATIVE: - return row->beaRelative; - + value = row->beaRelative; break; case ALTITUDERELATIVE: - return row->altitudeRelative; - + value = row->altitudeRelative; break; case ISRELATIVE: - return row->isRelative; - + value = row->isRelative; break; case ALTITUDE: - return row->altitude; - + value = row->altitude; break; case VELOCITY: - return row->velocity; - + value = row->velocity; break; case MODE: - return row->mode; - + value = row->mode; break; case MODE_PARAMS0: - return row->mode_params[0]; - + value = row->mode_params[0]; break; case MODE_PARAMS1: - return row->mode_params[1]; - + value = row->mode_params[1]; break; case MODE_PARAMS2: - return row->mode_params[2]; - + value = row->mode_params[2]; break; case MODE_PARAMS3: - return row->mode_params[3]; - + value = row->mode_params[3]; break; case CONDITION: - return row->condition; - + value = row->condition; break; case CONDITION_PARAMS0: - return row->condition_params[0]; - + value = row->condition_params[0]; break; case CONDITION_PARAMS1: - return row->condition_params[1]; - + value = row->condition_params[1]; break; case CONDITION_PARAMS2: - return row->condition_params[2]; - + value = row->condition_params[2]; break; case CONDITION_PARAMS3: - return row->condition_params[3]; - + value = row->condition_params[3]; break; case COMMAND: - return row->command; - + value = row->command; break; case JUMPDESTINATION: - return row->jumpdestination; - + value = row->jumpdestination; break; case ERRORDESTINATION: - return row->errordestination; - + value = row->errordestination; break; case LOCKED: - return row->locked; + value = row->locked; + break; } - return NULL; + return value; } QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const { + QVariant value; if (role == Qt::DisplayRole) { if (orientation == Qt::Vertical) { - return QString::number(section + 1); + value = QString::number(section + 1); } else if (orientation == Qt::Horizontal) { switch (section) { case WPDESCRITPTION: - return QString("Description"); - + value = QString("Description"); break; case LATPOSITION: - return QString("Latitude"); - + value = QString("Latitude"); break; case LNGPOSITION: - return QString("Longitude"); - + value = QString("Longitude"); break; case DISRELATIVE: - return QString("Distance to home"); - + value = QString("Distance to home"); break; case BEARELATIVE: - return QString("Bearing from home"); - + value = QString("Bearing from home"); break; case ALTITUDERELATIVE: - return QString("Altitude above home"); - + value = QString("Altitude above home"); break; case ISRELATIVE: - return QString("Relative to home"); - + value = QString("Relative to home"); break; case ALTITUDE: - return QString("Altitude"); - + value = QString("Altitude"); break; case VELOCITY: - return QString("Velocity"); - + value = QString("Velocity"); break; case MODE: - return QString("Mode"); - + value = QString("Mode"); break; case MODE_PARAMS0: - return QString("Mode parameter 0"); - + value = QString("Mode parameter 0"); break; case MODE_PARAMS1: - return QString("Mode parameter 1"); - + value = QString("Mode parameter 1"); break; case MODE_PARAMS2: - return QString("Mode parameter 2"); - + value = QString("Mode parameter 2"); break; case MODE_PARAMS3: - return QString("Mode parameter 3"); - + value = QString("Mode parameter 3"); break; case CONDITION: - return QString("Condition"); - + value = QString("Condition"); break; case CONDITION_PARAMS0: - return QString("Condition parameter 0"); - + value = QString("Condition parameter 0"); break; case CONDITION_PARAMS1: - return QString("Condition parameter 1"); - + value = QString("Condition parameter 1"); break; case CONDITION_PARAMS2: - return QString("Condition parameter 2"); - + value = QString("Condition parameter 2"); break; case CONDITION_PARAMS3: - return QString("Condition parameter 3"); - + value = QString("Condition parameter 3"); break; case COMMAND: - return QString("Command"); - + value = QString("Command"); break; case JUMPDESTINATION: - return QString("Jump Destination"); - + value = QString("Jump Destination"); break; case ERRORDESTINATION: - return QString("Error Destination"); - + value = QString("Error Destination"); break; case LOCKED: - return QString("Locked"); - + value = QString("Locked"); break; default: - return QString(); - + value = QString(); break; } } } else { - return QAbstractTableModel::headerData(section, orientation, role); + value = QAbstractTableModel::headerData(section, orientation, role); } - return NULL; + return value; } bool flightDataModel::setData(const QModelIndex &index, const QVariant &value, int role) @@ -665,54 +605,56 @@ void flightDataModel::readFromFile(QString fileName) while (!fieldNode.isNull()) { QDomElement field = fieldNode.toElement(); if (field.tagName() == "field") { - if (field.attribute("name") == "altitude") { - data->altitude = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "description") { - data->wpDescritption = field.attribute("value"); - } else if (field.attribute("name") == "latitude") { - data->latPosition = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "longitude") { - data->lngPosition = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "distance_to_home") { - data->disRelative = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "bearing_from_home") { - data->beaRelative = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "altitude_above_home") { - data->altitudeRelative = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "is_relative_to_home") { - data->isRelative = field.attribute("value").toInt(); - } else if (field.attribute("name") == "altitude") { - data->altitude = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "velocity") { - data->velocity = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "mode") { - data->mode = field.attribute("value").toInt(); - } else if (field.attribute("name") == "mode_param0") { - data->mode_params[0] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "mode_param1") { - data->mode_params[1] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "mode_param2") { - data->mode_params[2] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "mode_param3") { - data->mode_params[3] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "condition") { - data->condition = field.attribute("value").toDouble(); - } else if (field.attribute("name") == "condition_param0") { - data->condition_params[0] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "condition_param1") { - data->condition_params[1] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "condition_param2") { - data->condition_params[2] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "condition_param3") { - data->condition_params[3] = field.attribute("value").toFloat(); - } else if (field.attribute("name") == "command") { - data->command = field.attribute("value").toInt(); - } else if (field.attribute("name") == "jumpdestination") { - data->jumpdestination = field.attribute("value").toInt(); - } else if (field.attribute("name") == "errordestination") { - data->errordestination = field.attribute("value").toInt(); - } else if (field.attribute("name") == "is_locked") { - data->locked = field.attribute("value").toInt(); + QString name = field.attribute("name"); + QString value = field.attribute("value"); + if (name == "altitude") { + data->altitude = value.toDouble(); + } else if (name == "description") { + data->wpDescritption = value; + } else if (name == "latitude") { + data->latPosition = value.toDouble(); + } else if (name == "longitude") { + data->lngPosition = value.toDouble(); + } else if (name == "distance_to_home") { + data->disRelative = value.toDouble(); + } else if (name == "bearing_from_home") { + data->beaRelative = value.toDouble(); + } else if (name == "altitude_above_home") { + data->altitudeRelative = value.toFloat(); + } else if (name == "is_relative_to_home") { + data->isRelative = value.toInt(); + } else if (name == "altitude") { + data->altitude = value.toDouble(); + } else if (name == "velocity") { + data->velocity = value.toFloat(); + } else if (name == "mode") { + data->mode = value.toInt(); + } else if (name == "mode_param0") { + data->mode_params[0] = value.toFloat(); + } else if (name == "mode_param1") { + data->mode_params[1] = value.toFloat(); + } else if (name == "mode_param2") { + data->mode_params[2] = value.toFloat(); + } else if (name == "mode_param3") { + data->mode_params[3] = value.toFloat(); + } else if (name == "condition") { + data->condition = value.toDouble(); + } else if (name == "condition_param0") { + data->condition_params[0] = value.toFloat(); + } else if (name == "condition_param1") { + data->condition_params[1] = value.toFloat(); + } else if (name == "condition_param2") { + data->condition_params[2] = value.toFloat(); + } else if (name == "condition_param3") { + data->condition_params[3] = value.toFloat(); + } else if (name == "command") { + data->command = value.toInt(); + } else if (name == "jumpdestination") { + data->jumpdestination = value.toInt(); + } else if (name == "errordestination") { + data->errordestination = value.toInt(); + } else if (name == "is_locked") { + data->locked = value.toInt(); } } fieldNode = fieldNode.nextSibling(); From 8c7792b6e5e5572269df70a6e8834d954969c04e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 00:09:51 +0100 Subject: [PATCH 086/113] OP-1122 OP-1120 Added a new FligthPlanInfo uavobject - FligthPlanInfo contains the number of waypoints and action pathes - FligthPlanInfo contains a CRC of all waypoints and action pathes - The CRC is not yet implemented - These informations will be used to check the consistency of a flight plan --- flight/modules/PathPlanner/pathplanner.c | 2 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../src/plugins/opmap/modeluavoproxy.cpp | 127 ++++++++++++------ .../src/plugins/opmap/modeluavoproxy.h | 12 +- .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/flightplaninfo.xml | 14 ++ 6 files changed, 111 insertions(+), 47 deletions(-) create mode 100644 shared/uavobjectdefinition/flightplaninfo.xml diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 837e78608..ab2716d99 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -31,6 +31,7 @@ #include "openpilot.h" +#include "flightplaninfo.h" #include "flightstatus.h" #include "airspeedstate.h" #include "pathaction.h" @@ -99,6 +100,7 @@ int32_t PathPlannerInitialize() { taskHandle = NULL; + FlightPlanInfoInitialize(); PathActionInitialize(); PathStatusInitialize(); PathDesiredInitialize(); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 7a63dc9f2..153525147 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -43,6 +43,7 @@ UAVOBJSRCFILENAMES += debuglogentry UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplaninfo UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 5af2a707f..6d19d2248 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -26,6 +26,7 @@ */ #include "modeluavoproxy.h" #include "extensionsystem/pluginmanager.h" + #include ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) @@ -33,28 +34,32 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm != NULL); - objManager = pm->getObject(); - Q_ASSERT(objManager != NULL); + objMngr = pm->getObject(); + Q_ASSERT(objMngr != NULL); } void ModelUavoProxy::sendFlightPlan() { modelToObjects(); - Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); - connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr, 0); + connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); - PathAction *action = PathAction::GetInstance(objManager, 0); - connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objMngr, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); completionCount = 0; completionSuccessCount = 0; + flightPlan->updated(); waypoint->updatedAll(); action->updatedAll(); } -void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) +void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success) { obj->disconnect(this); @@ -63,9 +68,9 @@ void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) completionSuccessCount++; } - if (completionCount == 2) { - qDebug() << "ModelUavoProxy::flightPlanSent - success" << (completionSuccessCount == 2); - if (completionSuccessCount == 2) { + if (completionCount == 3) { + qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (completionSuccessCount == 3); + if (completionSuccessCount == 3) { QMessageBox::information(NULL, tr("Flight Plan Upload Successful"), tr("Flight plan upload was successful.")); } else { @@ -76,20 +81,24 @@ void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) void ModelUavoProxy::receiveFlightPlan() { - Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); - connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr, 0); + connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); - PathAction *action = PathAction::GetInstance(objManager, 0); - connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objMngr, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); completionCount = 0; completionSuccessCount = 0; + flightPlan->requestUpdate(); waypoint->requestUpdateAll(); action->requestUpdateAll(); } -void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) +void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success) { obj->disconnect(this); @@ -98,9 +107,9 @@ void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) completionSuccessCount++; } - if (completionCount == 2) { - qDebug() << "ModelUavoProxy::flightPlanReceived - success" << (completionSuccessCount == 2); - if (completionSuccessCount == 2) { + if (completionCount == 3) { + qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (completionSuccessCount == 3); + if (completionSuccessCount == 3) { objectsToModel(); QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); } @@ -110,23 +119,26 @@ void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) } } +// update waypoint and path actions UAV objects +// +// waypoints are unique and each waypoint has an entry in the UAV waypoint list +// +// a path action can be referenced by multiple waypoints +// waypoints reference path action by their index in the UAV path action list +// the compression of path actions happens here. +// (compression consists in keeping only one instance of similar path actions) +// +// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances void ModelUavoProxy::modelToObjects() { qDebug() << "ModelUAVProxy::modelToObjects"; - // update waypoint and path actions UAV objects - // waypoints are unique and each waypoint has an entry in the UAV waypoint list - // a path action can be referenced by multiple waypoints - // waypoints reference path action by their index in the UAV path action list - // the factoring of path actions (to remove duplicates) happens here... - - // the UAV waypoint list and path action list are probably not empty - // so we try to reuse existing instances // track number of path actions int actionCount = 0; // iterate over waypoints - for (int i = 0; i < myModel->rowCount(); ++i) { + int waypointCount = myModel->rowCount(); + for (int i = 0; i < waypointCount; ++i) { // Path Actions @@ -174,15 +186,38 @@ void ModelUavoProxy::modelToObjects() // update UAVObject waypoint->setData(waypointData); } + // Put "safe" values in unused waypoint objects + if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) { + for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) { + // TODO + } + } + // Put "safe" values in unused path action objects + if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) { + for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) { + // TODO + } + } + + // Update FlightPlanInfo + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr); + FlightPlanInfo::DataFields flightPlanData = flightPlan->getData(); + + flightPlanData.WaypointCount = waypointCount; + flightPlanData.PathActionCount = actionCount; + // TODO + flightPlanData.Crc = 0; + + flightPlan->setData(flightPlanData); } Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { Waypoint *waypoint = NULL; - int count = objManager->getNumInstances(Waypoint::OBJID); + int count = objMngr->getNumInstances(Waypoint::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; - waypoint = Waypoint::GetInstance(objManager, index); + waypoint = Waypoint::GetInstance(objMngr, index); if (newWaypoint) { newWaypoint->deleteLater(); } @@ -192,7 +227,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { // TODO is there a limit to the number of wp? waypoint = newWaypoint ? newWaypoint : new Waypoint; waypoint->initialize(index, waypoint->getMetaObject()); - objManager->registerObject(waypoint); + objMngr->registerObject(waypoint); } else { // we can only create the "next" object @@ -203,11 +238,11 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *action = NULL; - int count = objManager->getNumInstances(PathAction::OBJID); + int count = objMngr->getNumInstances(PathAction::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; - action = PathAction::GetInstance(objManager, index); + action = PathAction::GetInstance(objMngr, index); if (newAction) { newAction->deleteLater(); } @@ -217,7 +252,7 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { // TODO is there a limit to the number of path actions? action = newAction ? newAction : new PathAction; action->initialize(index, action->getMetaObject()); - objManager->registerObject(action); + objMngr->registerObject(action); } else { // we can only create the "next" object @@ -227,10 +262,10 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { } PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { - int instancesCount = objManager->getNumInstances(PathAction::OBJID); + int instancesCount = objMngr->getNumInstances(PathAction::OBJID); int count = actionCount <= instancesCount ? actionCount : instancesCount; for (int i = 0; i < count; ++i) { - PathAction *action = PathAction::GetInstance(objManager, i); + PathAction *action = PathAction::GetInstance(objMngr, i); Q_ASSERT(action); if (!action) { continue; @@ -258,18 +293,24 @@ void ModelUavoProxy::objectsToModel() // the list of objects can end with "garbage" instances due to previous flightpath // they need to be ignored - int instanceCount = objManager->getNumInstances(Waypoint::OBJID); + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr); + FlightPlanInfo::DataFields flightPlanData = flightPlan->getData(); + + int waypointCount = flightPlanData.WaypointCount; + + // TODO consistency checks + // objMngr->getNumInstances(Waypoint::OBJID); int rowCount = myModel->rowCount(); - if (instanceCount < rowCount) { - myModel->removeRows(instanceCount, rowCount - instanceCount); + if (waypointCount < rowCount) { + myModel->removeRows(waypointCount, rowCount - waypointCount); } - else if (instanceCount > rowCount) { - myModel->insertRows(rowCount, instanceCount - rowCount); + else if (waypointCount > rowCount) { + myModel->insertRows(rowCount, waypointCount - rowCount); } - for (int i = 0; i < instanceCount; ++i) { - Waypoint *waypoint = Waypoint::GetInstance(objManager, i); + for (int i = 0; i < waypointCount; ++i) { + Waypoint *waypoint = Waypoint::GetInstance(objMngr, i); Q_ASSERT(waypoint); if (!waypoint) { continue; @@ -278,7 +319,7 @@ void ModelUavoProxy::objectsToModel() Waypoint::DataFields waypointData = waypoint->getData(); waypointToModel(i, waypointData); - PathAction *action = PathAction::GetInstance(objManager, waypoint->getAction()); + PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction()); Q_ASSERT(action); if (!action) { continue; diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 9e3a2cd2e..5d1d5b942 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -27,11 +27,14 @@ #ifndef MODELUAVOPROXY_H #define MODELUAVOPROXY_H -#include #include "flightdatamodel.h" + +#include "flightplaninfo.h" #include "pathaction.h" #include "waypoint.h" +#include + class ModelUavoProxy : public QObject { Q_OBJECT @@ -43,8 +46,9 @@ public slots: void receiveFlightPlan(); private: - UAVObjectManager *objManager; + UAVObjectManager *objMngr; flightDataModel *myModel; + uint completionCount; uint completionSuccessCount; @@ -63,8 +67,8 @@ private: void pathActionToModel(int i, PathAction::DataFields &data); private slots: - void flightPlanSent(UAVObject *, bool success); - void flightPlanReceived(UAVObject *, bool success); + void flightPlanElementSent(UAVObject *, bool success); + void flightPlanElementReceived(UAVObject *, bool success); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index f65bffd4d..04be9ca02 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -92,6 +92,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.h \ $$UAVOBJECT_SYNTHETICS/taskinfo.h \ + $$UAVOBJECT_SYNTHETICS/flightplaninfo.h \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.h \ $$UAVOBJECT_SYNTHETICS/flightplansettings.h \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \ @@ -184,6 +185,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.cpp \ $$UAVOBJECT_SYNTHETICS/taskinfo.cpp \ + $$UAVOBJECT_SYNTHETICS/flightplaninfo.cpp \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.cpp \ $$UAVOBJECT_SYNTHETICS/flightplansettings.cpp \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \ diff --git a/shared/uavobjectdefinition/flightplaninfo.xml b/shared/uavobjectdefinition/flightplaninfo.xml new file mode 100644 index 000000000..05727e50f --- /dev/null +++ b/shared/uavobjectdefinition/flightplaninfo.xml @@ -0,0 +1,14 @@ + + + Flight plan informations + + + + + + + + + + + From f422a1d639c9a2657c7c350466833fec3a371b6f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 00:35:32 +0100 Subject: [PATCH 087/113] OP-1122 OP-1145 made ground uavtalk code more similar to its flight side version --- .../src/plugins/uavtalk/uavtalk.cpp | 72 +++++++++++-------- .../src/plugins/uavtalk/uavtalk.h | 4 +- 2 files changed, 43 insertions(+), 33 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 3a2acb459..52b32264a 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -89,7 +89,6 @@ UAVTalk::~UAVTalk() closeAllTransactions(); } - /** * Reset the statistics counters */ @@ -226,8 +225,31 @@ void UAVTalk::processInputStream() if (io && io->isReadable()) { while (io->bytesAvailable() > 0) { - io->read((char *)&tmp, 1); - processInputByte(tmp); + int ret = io->read((char *)&tmp, 1); + if (ret != -1) { + processInputByte(tmp); + } + else { + // TODOD + } + if (rxState == STATE_COMPLETE) { + mutex.lock(); + if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) { + stats.rxObjectBytes += rxLength; + stats.rxObjects++; + } + else { + // TODO... + } + mutex.unlock(); + + if (useUDPMirror) { + // it is safe to do this outside of the above critical section as the rxDataArray is + // accessed from this thread only + udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); + } + + } } } } @@ -239,6 +261,14 @@ void UAVTalk::processInputStream() */ bool UAVTalk::processInputByte(quint8 rxbyte) { + if (rxState == STATE_COMPLETE || rxState == STATE_ERROR) { + rxState = STATE_SYNC; + + if (useUDPMirror) { + rxDataArray.clear(); + } + } + // Update stats stats.rxBytes++; @@ -267,11 +297,6 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // case local byte counter, don't forget to zero it after use. rxCount = 0; - if (useUDPMirror) { - rxDataArray.clear(); - rxDataArray.append(rxbyte); - } - rxState = STATE_TYPE; break; @@ -283,7 +308,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if ((rxbyte & TYPE_MASK) != TYPE_VER) { qWarning() << "UAVTalk - error : bad type"; stats.rxErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } @@ -312,7 +337,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // incorrect packet size qWarning() << "UAVTalk - error : incorrect packet size"; stats.rxErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } @@ -356,7 +381,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxObj == NULL && rxType != TYPE_OBJ_REQ) { qWarning() << "UAVTalk - error : unknown object" << rxObjId; stats.rxErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } @@ -376,7 +401,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // packet error - exceeded payload max length qWarning() << "UAVTalk - error : exceeded payload max length" << rxObjId; stats.rxErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } @@ -385,7 +410,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // packet error - mismatched packet size qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId; stats.rxErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } @@ -422,7 +447,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // packet error - faulty CRC qWarning() << "UAVTalk - error : failed CRC check" << rxObjId; stats.rxCrcErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } @@ -430,30 +455,17 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // packet error - mismatched packet size qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId; stats.rxErrors++; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } - mutex.lock(); - - receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); - - stats.rxObjectBytes += rxLength; - stats.rxObjects++; - - mutex.unlock(); - - if (useUDPMirror) { - udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); - } - - rxState = STATE_SYNC; + rxState = STATE_COMPLETE; break; default: qWarning() << "UAVTalk - error : bad state"; - rxState = STATE_SYNC; + rxState = STATE_ERROR; break; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 87648a4f5..858fa3b9e 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -109,7 +109,7 @@ private: // Types typedef enum { - STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS + STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS, STATE_COMPLETE, STATE_ERROR } RxStateType; // Variables @@ -123,8 +123,6 @@ private: QMap *> transMap; -// quint16 rxReadOffset; -// quint16 rxSyncOffset; quint8 rxBuffer[MAX_PACKET_LENGTH]; quint8 txBuffer[MAX_PACKET_LENGTH]; From e716d97803dd14b221fbcfeffae9070cbcdf8dd6 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 01:10:18 +0100 Subject: [PATCH 088/113] OP-1122 OP-1120 moved CRC8 utility from uavtalk.cpp to new utils/crc.cpp --- ground/openpilotgcs/src/libs/utils/crc.cpp | 74 +++++++++++++++++++ ground/openpilotgcs/src/libs/utils/crc.h | 62 ++++++++++++++++ ground/openpilotgcs/src/libs/utils/utils.pro | 6 +- .../src/plugins/uavtalk/uavtalk.cpp | 65 +++------------- .../src/plugins/uavtalk/uavtalk.h | 2 - 5 files changed, 149 insertions(+), 60 deletions(-) create mode 100644 ground/openpilotgcs/src/libs/utils/crc.cpp create mode 100644 ground/openpilotgcs/src/libs/utils/crc.h diff --git a/ground/openpilotgcs/src/libs/utils/crc.cpp b/ground/openpilotgcs/src/libs/utils/crc.cpp new file mode 100644 index 000000000..2d66e1dcb --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/crc.cpp @@ -0,0 +1,74 @@ +/** + ****************************************************************************** + * + * @file crc.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup CorePlugin Core Plugin + * @{ + * @brief The Core GCS plugin + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "crc.h" + +using namespace Utils; + +/* + * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/ + * using the configuration: + * Width = 8 + * Poly = 0x07 + * XorIn = 0x00 + * ReflectIn = False + * XorOut = 0x00 + * ReflectOut = False + * Algorithm = table-driven + */ +const quint8 crc_table[256] = { + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 +}; + +quint8 Crc::updateCRC(quint8 crc, const quint8 data) +{ + return crc_table[crc ^ data]; +} + +quint8 Crc::updateCRC(quint8 crc, const quint8 *data, qint32 length) +{ + while (length--) { + crc = crc_table[crc ^ *data++]; + } + return crc; +} diff --git a/ground/openpilotgcs/src/libs/utils/crc.h b/ground/openpilotgcs/src/libs/utils/crc.h new file mode 100644 index 000000000..2d9fc15a1 --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/crc.h @@ -0,0 +1,62 @@ +/** + ****************************************************************************** + * + * @file crc.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup CorePlugin Core Plugin + * @{ + * @brief The Core GCS plugin + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef CRC_H +#define CRC_H + +#include "utils_global.h" + +namespace Utils { + +class QTCREATOR_UTILS_EXPORT Crc { + +public: + /** + * Update the crc value with new data. + * + * \param crc The current crc value. + * \param data data to update the crc with. + * \return The updated crc value. + */ + static quint8 updateCRC(quint8 crc, const quint8 data); + + /** + * Update the crc value with new data. + * + * \param crc The current crc value. + * \param data Pointer to a buffer of \a data_len bytes. + * \param length Number of bytes in the \a data buffer. + * \return The updated crc value. + */ + static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); + +}; + +} // namespace Utils + +#endif // CRC_H diff --git a/ground/openpilotgcs/src/libs/utils/utils.pro b/ground/openpilotgcs/src/libs/utils/utils.pro index 311212891..231ff64f0 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pro +++ b/ground/openpilotgcs/src/libs/utils/utils.pro @@ -55,7 +55,8 @@ SOURCES += reloadpromptutils.cpp \ cachedsvgitem.cpp \ svgimageprovider.cpp \ hostosinfo.cpp \ - logfile.cpp + logfile.cpp \ + crc.cpp SOURCES += xmlconfig.cpp @@ -113,7 +114,8 @@ HEADERS += utils_global.h \ cachedsvgitem.h \ svgimageprovider.h \ hostosinfo.h \ - logfile.h + logfile.h \ + crc.h HEADERS += xmlconfig.h diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 52b32264a..890922116 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -27,6 +27,7 @@ #include "uavtalk.h" #include #include +#include #include #include @@ -38,24 +39,7 @@ #define SYNC_VAL 0x3C -const quint8 UAVTalk::crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 -}; +using namespace Utils; /** * Constructor @@ -290,7 +274,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) } // Initialize and update CRC - rxCS = updateCRC(0, rxbyte); + rxCS = Crc::updateCRC(0, rxbyte); rxPacketLength = 1; @@ -303,7 +287,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_TYPE: // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxCS = Crc::updateCRC(rxCS, rxbyte); if ((rxbyte & TYPE_MASK) != TYPE_VER) { qWarning() << "UAVTalk - error : bad type"; @@ -322,7 +306,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_SIZE: // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxCS = Crc::updateCRC(rxCS, rxbyte); if (rxCount == 0) { packetSize += rxbyte; @@ -347,7 +331,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_OBJID: // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxCS = Crc::updateCRC(rxCS, rxbyte); rxTmpBuffer[rxCount++] = rxbyte; if (rxCount < 4) { @@ -365,7 +349,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_INSTID: // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxCS = Crc::updateCRC(rxCS, rxbyte); rxTmpBuffer[rxCount++] = rxbyte; if (rxCount < 2) { @@ -427,7 +411,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_DATA: // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxCS = Crc::updateCRC(rxCS, rxbyte); rxBuffer[rxCount++] = rxbyte; if (rxCount < rxLength) { @@ -802,7 +786,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U qToLittleEndian(HEADER_LENGTH + length, &txBuffer[2]); // Calculate checksum - txBuffer[HEADER_LENGTH + length] = updateCRC(0, txBuffer, HEADER_LENGTH + length); + txBuffer[HEADER_LENGTH + length] = Crc::updateCRC(0, txBuffer, HEADER_LENGTH + length); // Send buffer, check that the transmit backlog does not grow above limit if (!io.isNull() && io->isWritable()) { @@ -833,37 +817,6 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U return true; } -/** - * Update the crc value with new data. - * - * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/ - * using the configuration: - * Width = 8 - * Poly = 0x07 - * XorIn = 0x00 - * ReflectIn = False - * XorOut = 0x00 - * ReflectOut = False - * Algorithm = table-driven - * - * \param crc The current crc value. - * \param data Pointer to a buffer of \a data_len bytes. - * \param length Number of bytes in the \a data buffer. - * \return The updated crc value. - */ -quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data) -{ - return crc_table[crc ^ data]; -} - -quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length) -{ - while (length--) { - crc = crc_table[crc ^ *data++]; - } - return crc; -} - UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId) { // Lookup the transaction in the transaction map diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 858fa3b9e..fe5922096 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -156,8 +156,6 @@ private: void updateNack(quint32 objId, quint16 instId, UAVObject *obj); bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); - quint8 updateCRC(quint8 crc, const quint8 data); - quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); Transaction *findTransaction(quint32 objId, quint16 instId); void openTransaction(quint8 type, quint32 objId, quint16 instId); From bcb59a3dcae0562ca1ceb2ada3a037f04340e89b Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 21:46:50 +0100 Subject: [PATCH 089/113] OP-1122 OP-1120 Added missing FligthPlanInfo uavobject to revoproto/firmware/UAVObjects.inc --- flight/targets/boards/revoproto/firmware/UAVObjects.inc | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 80dab9e61..ad22cf2a7 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -44,6 +44,7 @@ UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplaninfo UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus From 48aa89d2e704cce18788b739cf5d1103d536ceb4 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 21:48:27 +0100 Subject: [PATCH 090/113] OP-1122 OP-1120 OPLinkReceiver uavobject had no caterogy defined (is now part of the System category) --- shared/uavobjectdefinition/oplinkreceiver.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/oplinkreceiver.xml b/shared/uavobjectdefinition/oplinkreceiver.xml index 96e3187af..5323d1522 100644 --- a/shared/uavobjectdefinition/oplinkreceiver.xml +++ b/shared/uavobjectdefinition/oplinkreceiver.xml @@ -1,5 +1,5 @@ - + A receiver channel group carried over the OPLink radio. From ca607ad924151be2019b9c4715aa4f024a8c32b7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 11 Jan 2014 10:51:47 +0100 Subject: [PATCH 091/113] OP-1022 overhaul of altitudehold and manualcontrol - cleanup, reimplemented safe throttle cutoff and altitudevario features --- flight/modules/AltitudeHold/altitudehold.c | 65 +++++++---- flight/modules/ManualControl/manualcontrol.c | 109 ++++++++---------- .../altitudeholddesired.xml | 6 +- .../altitudeholdsettings.xml | 1 + 4 files changed, 96 insertions(+), 85 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index f177d9c8b..6a81518d6 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -146,35 +146,54 @@ static void altitudeHoldTask(void) float velocityStateDown; VelocityStateDownGet(&velocityStateDown); - // altitude control loop - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + switch (altitudeHoldDesired.ThrottleMode) { + case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE: + // altitude control loop + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.ThrottleCommand, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + break; + case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY: + altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand; + break; + default: + altitudeHoldStatus.VelocityDesired = 0; + break; + } AltitudeHoldStatusSet(&altitudeHoldStatus); - // velocity control loop - float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + float throttle; + switch (altitudeHoldDesired.ThrottleMode) { + case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE: + throttle = altitudeHoldDesired.ThrottleCommand; + break; + default: + // velocity control loop + throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); - // compensate throttle for current attitude - // Rbe[2][2] in the rotation matrix is the rotated down component of a - // 0,0,1 vector - // it is used to scale the throttle, but only up to 4 times the regular - // throttle - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rbe[3][3]; - Quaternion2R(&attitudeState.q1, Rbe); - if (fabsf(Rbe[2][2]) > 0.25f) { - throttle /= Rbe[2][2]; - } else { - throttle /= 0.25f; + // compensate throttle for current attitude + // Rbe[2][2] in the rotation matrix is the rotated down component of a + // 0,0,1 vector + // it is used to scale the throttle, but only up to 4 times the regular + // throttle + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1, Rbe); + if (fabsf(Rbe[2][2]) > 0.25f) { + throttle /= Rbe[2][2]; + } else { + throttle /= 0.25f; + } + + if (throttle >= 1.0f) { + throttle = 1.0f; + } + if (throttle <= 0.0f) { + throttle = 0.0f; + } + break; } - if (throttle >= 1.0f) { - throttle = 1.0f; - } - if (throttle <= 0.0f) { - throttle = 0.0f; - } StabilizationDesiredData stab; StabilizationDesiredGet(&stab); stab.Roll = altitudeHoldDesired.Roll; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index a9882966e..dbc7ecc03 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -691,26 +691,26 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont } stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode @@ -722,18 +722,17 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - } - else { + } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode } @@ -850,30 +849,20 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - // Stop updating AltitudeHoldDesired triggering a failsafe condition. - if (cmd->Throttle < 0) { - return; - } - // this is the max speed in m/s at the extents of throttle uint8_t throttleRate; uint8_t throttleExp; static uint8_t flightMode; - static bool zeroed = false; + static bool newaltitude = true; FlightStatusFlightModeGet(&flightMode); AltitudeHoldDesiredData altitudeHoldDesiredData; AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { - throttleExp = 128; - throttleRate = 0; - } else { - AltitudeHoldSettingsThrottleExpGet(&throttleExp); - AltitudeHoldSettingsThrottleRateGet(&throttleRate); - } + AltitudeHoldSettingsThrottleExpGet(&throttleExp); + AltitudeHoldSettingsThrottleRateGet(&throttleRate); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); @@ -881,33 +870,35 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) PositionStateData posState; PositionStateGet(&posState); - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; if (changed) { - // After not being in this mode for a while init at current height - altitudeHoldDesiredData.Velocity = 0; - altitudeHoldDesiredData.Altitude = posState.Down; - zeroed = false; - } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { + newaltitude = true; + } + + uint8_t cutOff; + AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); + if (cutOff && cmd->Throttle < 0) { + // Cut throttle if desired + altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle; + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); - altitudeHoldDesiredData.Altitude = posState.Down; - } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); - altitudeHoldDesiredData.Altitude = posState.Down; - } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { - // Require the stick to enter the dead band before they can move height - // Vario is not "engaged" when throttleRate == 0 - if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { - ; - altitudeHoldDesiredData.Velocity = 0; - altitudeHoldDesiredData.Altitude = posState.Down; - } - zeroed = true; + altitudeHoldDesiredData.ThrottleCommand = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { + altitudeHoldDesiredData.ThrottleCommand = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + newaltitude = true; + } else if (newaltitude == true) { + altitudeHoldDesiredData.ThrottleCommand = posState.Down; + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE; + newaltitude = false; } AltitudeHoldDesiredSet(&altitudeHoldDesiredData); diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 7a69227fd..0dfe36ceb 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,9 +1,9 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through - - - + + + diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 61724dc35..8e88100f1 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -3,6 +3,7 @@ Settings for the @ref AltitudeHold module + From 879e29b6cd4a2fb9b47bab9c65f37e80175f0450 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 11 Jan 2014 14:12:02 +0100 Subject: [PATCH 092/113] OP-1022 Changed altitude (vario) complementary filter to pay more attention to barometer and apply more filtering to accelerometer (needed to compensate vibration noise and errors on flying quads) --- shared/uavobjectdefinition/altitudefiltersettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/altitudefiltersettings.xml b/shared/uavobjectdefinition/altitudefiltersettings.xml index bc6222ad1..9b3eb0838 100644 --- a/shared/uavobjectdefinition/altitudefiltersettings.xml +++ b/shared/uavobjectdefinition/altitudefiltersettings.xml @@ -1,9 +1,9 @@ Settings for the @ref State Estimator module plugin altitudeFilter - + - + From 78fabdd4c3feda9fe986a7f73288d62235e0b14b Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 12 Jan 2014 15:08:12 +0100 Subject: [PATCH 093/113] OP-1122 OP-1158 added facility to update a CRC with uavobject data (ground side) --- .../src/plugins/uavobjects/uavobject.cpp | 26 +++++++++++++++++-- .../src/plugins/uavobjects/uavobject.h | 1 + 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index a43f893ef..275a9708b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -26,9 +26,14 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobject.h" + +#include + #include #include +using namespace Utils; + // Constants #define UAVOBJ_ACCESS_SHIFT 0 #define UAVOBJ_GCS_ACCESS_SHIFT 1 @@ -54,6 +59,8 @@ UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name) this->instID = 0; this->isSingleInst = isSingleInst; this->name = name; + this->data = 0; + this->numBytes = 0; this->mutex = new QMutex(QMutex::Recursive); } @@ -163,7 +170,6 @@ void UAVObject::setCategory(const QString & category) this->category = category; } - /** * Get the total number of bytes of the object's data */ @@ -280,7 +286,8 @@ UAVObjectField *UAVObject::getField(const QString & name) } } // If this point is reached then the field was not found - qWarning() << "UAVObject::getField Non existant field" << name << "requested. This indicates a bug. Make sure you also have null checking for non-debug code."; + qWarning() << "UAVObject::getField Non existant field" << name << "requested." + << "This indicates a bug. Make sure you also have null checking for non-debug code."; return NULL; } @@ -319,6 +326,21 @@ qint32 UAVObject::unpack(const quint8 *dataIn) return numBytes; } +/** + * Update a CRC with the object data + * @returns The updated CRC + */ +quint8 UAVObject::updateCRC(quint8 crc) +{ + QMutexLocker locker(mutex); + + //crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID)); + //crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID)); + crc = Crc::updateCRC(crc, data, numBytes); + + return crc; +} + /** * Save the object data to the file. * The file will be created in the current directory diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h index 4217992bf..9c34271c0 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h @@ -109,6 +109,7 @@ public: quint32 getNumBytes(); qint32 pack(quint8 *dataOut); qint32 unpack(const quint8 *dataIn); + quint8 updateCRC(quint8 crc = 0); bool save(); bool save(QFile & file); bool load(); From d8d9adb1fd51ba36c7e8deed3bbd5e394d3be100 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 12 Jan 2014 15:10:15 +0100 Subject: [PATCH 094/113] OP-1122 OP-1158 remove unused flightplan.h (fixes conflict with new FlightPlan uavobject) --- flight/modules/FlightPlan/flightplan.c | 1 - flight/modules/FlightPlan/inc/flightplan.h | 36 ---------------------- 2 files changed, 37 deletions(-) delete mode 100644 flight/modules/FlightPlan/inc/flightplan.h diff --git a/flight/modules/FlightPlan/flightplan.c b/flight/modules/FlightPlan/flightplan.c index 0341263cb..b53b76ec9 100644 --- a/flight/modules/FlightPlan/flightplan.c +++ b/flight/modules/FlightPlan/flightplan.c @@ -31,7 +31,6 @@ #include -#include "flightplan.h" #include "flightplanstatus.h" #include "flightplancontrol.h" #include "flightplansettings.h" diff --git a/flight/modules/FlightPlan/inc/flightplan.h b/flight/modules/FlightPlan/inc/flightplan.h deleted file mode 100644 index dc51cca7f..000000000 --- a/flight/modules/FlightPlan/inc/flightplan.h +++ /dev/null @@ -1,36 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup FlightPlan Flight Plan Module - * @brief Executes flight plan scripts in Python - * @{ - * - * @file flightplan.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Executes flight plan scripts in Python - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ -#ifndef FLIGHTPLAN_H -#define FLIGHTPLAN_H - -int32_t FlightPlanInitialize(); - -#endif // FLIGHTPLAN_H From c8a81f88a445f7386eac42e9a673fa513cb0fe34 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 12 Jan 2014 15:12:41 +0100 Subject: [PATCH 095/113] OP-1122 OP-1158 added facility to update a CRC with uavobject data (flight side) --- flight/pios/common/pios_crc.c | 22 +++++----- flight/uavobjects/inc/uavobjectmanager.h | 1 + flight/uavobjects/uavobjectmanager.c | 53 ++++++++++++++++++++---- 3 files changed, 56 insertions(+), 20 deletions(-) diff --git a/flight/pios/common/pios_crc.c b/flight/pios/common/pios_crc.c index 9b2128f06..7cf8813f3 100644 --- a/flight/pios/common/pios_crc.c +++ b/flight/pios/common/pios_crc.c @@ -64,6 +64,17 @@ static const uint16_t CRC_Table16[] = { // HDLC polynomial 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 }; +/** + * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/ + * using the configuration: + * Width = 8 + * Poly = 0x07 + * XorIn = 0x00 + * ReflectIn = False + * XorOut = 0x00 + * ReflectOut = False + * Algorithm = table-driven + */ static const uint32_t CRC_Table32[] = { 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, @@ -101,17 +112,6 @@ static const uint32_t CRC_Table32[] = { /** * Update the crc value with new data. - * - * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/ - * using the configuration: - * Width = 8 - * Poly = 0x07 - * XorIn = 0x00 - * ReflectIn = False - * XorOut = 0x00 - * ReflectOut = False - * Algorithm = table-driven - * * \param crc The current crc value. * \param data Pointer to a buffer of \a data_len bytes. * \param length Number of bytes in the \a data buffer. diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index 9da465cba..c3813ef74 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/flight/uavobjects/inc/uavobjectmanager.h @@ -160,6 +160,7 @@ bool UAVObjIsMetaobject(UAVObjHandle obj); bool UAVObjIsSettings(UAVObjHandle obj); int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn); int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut); +uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc); int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId); diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 2a02b3ee1..bf5d52a80 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -171,17 +171,14 @@ struct UAVOMulti { /** all information about instances are dependant on object type **/ #define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0))) #define InstanceDataOffset(inst) ((void *)&(((struct UAVOMultiInst *)inst)->instance)) -#define InstanceData(instance) (void *)instance +#define InstanceData(instance) ((void *)instance) // Private functions -static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, - UAVObjEventType event); +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType event); static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId); static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId); -static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb, uint8_t eventMask); -static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb); +static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask); +static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb); static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId); // Private variables @@ -615,8 +612,7 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle) * \param[in] dataIn The byte array * \return 0 if success or -1 if failure */ -int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, - const uint8_t *dataIn) +int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn) { PIOS_Assert(obj_handle); @@ -704,6 +700,45 @@ unlock_exit: return rc; } +/** + * Update a CRC with an object data + * \param[in] obj The object handle + * \param[in] instId The instance ID + * \param[in] crc The crc to update + * \return the updated crc + */ +uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc) +{ + PIOS_Assert(obj_handle); + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + // TODO + } else { + struct UAVOData *obj; + InstanceHandle instEntry; + + // Cast handle to object + obj = (struct UAVOData *)obj_handle; + + // Get the instance + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Update crc + crc = PIOS_CRC_updateCRC(crc, (uint8_t *) InstanceData(instEntry), (int32_t) obj->instance_size); + } + +unlock_exit: + xSemaphoreGiveRecursive(mutex); + return crc; +} /** * Actually write the object's data to the logfile From aa24f2193a42b375bc759e4f6fa3b7a2f796fbfd Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 12 Jan 2014 15:20:22 +0100 Subject: [PATCH 096/113] OP-1122 OP-1158 renamed FlightPlanInfo uavobject to FlightPlan --- flight/modules/PathPlanner/pathplanner.c | 10 ++++++---- .../boards/revolution/firmware/UAVObjects.inc | 2 +- .../boards/revoproto/firmware/UAVObjects.inc | 2 +- .../src/plugins/opmap/modeluavoproxy.cpp | 14 +++++++------- .../src/plugins/opmap/modeluavoproxy.h | 2 +- .../src/plugins/uavobjects/uavobjects.pro | 18 ++++++++++++------ .../{flightplaninfo.xml => flightplan.xml} | 2 +- 7 files changed, 29 insertions(+), 21 deletions(-) rename shared/uavobjectdefinition/{flightplaninfo.xml => flightplan.xml} (85%) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index ab2716d99..f62e0e2af 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -31,7 +31,7 @@ #include "openpilot.h" -#include "flightplaninfo.h" +#include "flightplan.h" #include "flightstatus.h" #include "airspeedstate.h" #include "pathaction.h" @@ -100,7 +100,7 @@ int32_t PathPlannerInitialize() { taskHandle = NULL; - FlightPlanInfoInitialize(); + FlightPlanInitialize(); PathActionInitialize(); PathStatusInitialize(); PathDesiredInitialize(); @@ -269,6 +269,8 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) static void setWaypoint(uint16_t num) { // path plans wrap around + + // TODO change to FlightPlan.WaypointCount if (num >= UAVObjGetNumInstances(WaypointHandle())) { num = 0; } @@ -277,10 +279,10 @@ static void setWaypoint(uint16_t num) WaypointActiveSet(&waypointActive); } -// execute the apropriate condition and report result +// execute the appropriate condition and report result static uint8_t pathConditionCheck() { - // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's + // i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's switch (pathAction.EndCondition) { case PATHACTION_ENDCONDITION_NONE: return conditionNone(); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 153525147..337a2123a 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -43,7 +43,7 @@ UAVOBJSRCFILENAMES += debuglogentry UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate -UAVOBJSRCFILENAMES += flightplaninfo +UAVOBJSRCFILENAMES += flightplan UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index ad22cf2a7..d0480fa26 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -44,7 +44,7 @@ UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate -UAVOBJSRCFILENAMES += flightplaninfo +UAVOBJSRCFILENAMES += flightplan UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 6d19d2248..d6e9945f1 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -42,7 +42,7 @@ void ModelUavoProxy::sendFlightPlan() { modelToObjects(); - FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr, 0); + FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr, 0); connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); @@ -81,7 +81,7 @@ void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success) void ModelUavoProxy::receiveFlightPlan() { - FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr, 0); + FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr, 0); connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); @@ -199,9 +199,9 @@ void ModelUavoProxy::modelToObjects() } } - // Update FlightPlanInfo - FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr); - FlightPlanInfo::DataFields flightPlanData = flightPlan->getData(); + // Update FlightPlan + FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr); + FlightPlan::DataFields flightPlanData = flightPlan->getData(); flightPlanData.WaypointCount = waypointCount; flightPlanData.PathActionCount = actionCount; @@ -293,8 +293,8 @@ void ModelUavoProxy::objectsToModel() // the list of objects can end with "garbage" instances due to previous flightpath // they need to be ignored - FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr); - FlightPlanInfo::DataFields flightPlanData = flightPlan->getData(); + FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr); + FlightPlan::DataFields flightPlanData = flightPlan->getData(); int waypointCount = flightPlanData.WaypointCount; diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 5d1d5b942..23e81557b 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -29,7 +29,7 @@ #include "flightdatamodel.h" -#include "flightplaninfo.h" +#include "flightplan.h" #include "pathaction.h" #include "waypoint.h" diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 04be9ca02..4e0153d11 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -1,10 +1,13 @@ TEMPLATE = lib TARGET = UAVObjects + DEFINES += UAVOBJECTS_LIBRARY + include(../../openpilotgcsplugin.pri) include(uavobjects_dependencies.pri) -HEADERS += uavobjects_global.h \ +HEADERS += \ + uavobjects_global.h \ uavobject.h \ uavmetaobject.h \ uavobjectmanager.h \ @@ -14,7 +17,8 @@ HEADERS += uavobjects_global.h \ uavobjectsplugin.h \ uavobjecthelper.h -SOURCES += uavobject.cpp \ +SOURCES += \ + uavobject.cpp \ uavmetaobject.cpp \ uavobjectmanager.cpp \ uavdataobject.cpp \ @@ -25,7 +29,8 @@ SOURCES += uavobject.cpp \ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files -HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ +HEADERS += \ + $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/barosensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ @@ -92,7 +97,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.h \ $$UAVOBJECT_SYNTHETICS/taskinfo.h \ - $$UAVOBJECT_SYNTHETICS/flightplaninfo.h \ + $$UAVOBJECT_SYNTHETICS/flightplan.h \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.h \ $$UAVOBJECT_SYNTHETICS/flightplansettings.h \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \ @@ -118,7 +123,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h -SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ +SOURCES += \ + $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/barosensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ @@ -185,7 +191,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.cpp \ $$UAVOBJECT_SYNTHETICS/taskinfo.cpp \ - $$UAVOBJECT_SYNTHETICS/flightplaninfo.cpp \ + $$UAVOBJECT_SYNTHETICS/flightplan.cpp \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.cpp \ $$UAVOBJECT_SYNTHETICS/flightplansettings.cpp \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \ diff --git a/shared/uavobjectdefinition/flightplaninfo.xml b/shared/uavobjectdefinition/flightplan.xml similarity index 85% rename from shared/uavobjectdefinition/flightplaninfo.xml rename to shared/uavobjectdefinition/flightplan.xml index 05727e50f..e8c7b61c9 100644 --- a/shared/uavobjectdefinition/flightplaninfo.xml +++ b/shared/uavobjectdefinition/flightplan.xml @@ -1,5 +1,5 @@ - + Flight plan informations From a64720a9f13f361cbf87b8f2b58e3780e43f80ec Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 12 Jan 2014 19:42:12 +0100 Subject: [PATCH 097/113] OP-1122 OP-1158 added flight plan CRC consistency checks (in GCS and fixedwingpathfollower module) - need to do the same in vtolpathfollower module) - addressed an issue that could cause unwanted waypoints to be used (path follower was looping over all instanciated wp) --- .../fixedwingpathfollower.c | 57 ++++++++++- flight/modules/PathPlanner/pathplanner.c | 9 +- .../src/plugins/opmap/modeluavoproxy.cpp | 99 ++++++++++++------- .../src/plugins/opmap/modeluavoproxy.h | 11 ++- 4 files changed, 133 insertions(+), 43 deletions(-) diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 5858bf5e0..3226d80bd 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -45,7 +45,13 @@ #include +#include +#include + #include "hwsettings.h" +#include "flightplan.h" +#include "waypoint.h" +#include "pathaction.h" #include "attitudestate.h" #include "pathdesired.h" // object that will be updated by the module #include "positionstate.h" @@ -62,7 +68,6 @@ #include "velocitydesired.h" #include "velocitystate.h" #include "taskinfo.h" -#include #include "paths.h" #include "CoordinateConversions.h" @@ -80,6 +85,7 @@ static PathStatusData pathStatus; static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; // Private functions +static bool checkFlightPlan(); static void pathfollowerTask(void *parameters); static void SettingsUpdatedCb(UAVObjEvent *ev); static void updatePathVelocity(); @@ -163,6 +169,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + // 3. FlightPlan must be valid SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && @@ -172,11 +179,15 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) vTaskDelay(1000); continue; } + if (!checkFlightPlan()) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + vTaskDelay(1000); + continue; + } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); - FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); @@ -242,6 +253,48 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) } } +static bool checkFlightPlan() +{ + uint16_t i; + uint16_t waypointCount; + uint16_t actionCount; + uint8_t flightCrc; + FlightPlanData flightPlan; + + FlightPlanGet(&flightPlan); + + waypointCount = flightPlan.WaypointCount; + actionCount = flightPlan.PathActionCount; + + // check count consistency + if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { + PIOS_DEBUGLOG_Printf("FlighPlan : waypoint count error!"); + return false; + } + if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { + PIOS_DEBUGLOG_Printf("FlighPlan : path action count error!"); + return false; + } + + // check CRC + flightCrc = 0; + for(i = 0; i < waypointCount; i++) { + flightCrc = UAVObjUpdateCRC(WaypointHandle(), i, flightCrc); + } + for(i = 0; i < actionCount; i++) { + flightCrc = UAVObjUpdateCRC(PathActionHandle(), i, flightCrc); + } + if (flightCrc != flightPlan.Crc) { + PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", flightCrc, flightPlan.Crc); + return false; + } + + // everything ok (hopefully...) + PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", flightCrc, flightPlan.Crc); + + return true; +} + /** * Compute desired velocity from the current position and path * diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index f62e0e2af..eb43352c2 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -268,10 +268,13 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) // helper function to go to a specific waypoint static void setWaypoint(uint16_t num) { - // path plans wrap around + FlightPlanData flightPlan; - // TODO change to FlightPlan.WaypointCount - if (num >= UAVObjGetNumInstances(WaypointHandle())) { + FlightPlanGet(&flightPlan); + + // here it is assumed that the flight plan has been validated (waypoint count is consistent) + if (num >= flightPlan.WaypointCount) { + // path plans wrap around num = 0; } diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index d6e9945f1..dfa6b4818 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -36,6 +36,9 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec objMngr = pm->getObject(); Q_ASSERT(objMngr != NULL); + + completionCountdown = 0; + successCountdown = 0; } void ModelUavoProxy::sendFlightPlan() @@ -43,16 +46,20 @@ void ModelUavoProxy::sendFlightPlan() modelToObjects(); FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr, 0); - connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); + connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), + this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); - connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), + this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); PathAction *action = PathAction::GetInstance(objMngr, 0); - connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), + this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); - completionCount = 0; - completionSuccessCount = 0; + // we will start 3 update all + completionCountdown = 3; + successCountdown = completionCountdown; flightPlan->updated(); waypoint->updatedAll(); @@ -63,18 +70,16 @@ void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success) { obj->disconnect(this); - completionCount++; - if (success) { - completionSuccessCount++; - } + completionCountdown--; + successCountdown -= success ? 1 : 0; - if (completionCount == 3) { - qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (completionSuccessCount == 3); - if (completionSuccessCount == 3) { + if (completionCountdown == 0) { + qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (successCountdown == 0); + if (successCountdown == 0) { QMessageBox::information(NULL, tr("Flight Plan Upload Successful"), tr("Flight plan upload was successful.")); } else { - QMessageBox::critical(NULL, tr("Flight Plan Upload Failed !"), tr("Failed to upload the flight plan !")); + QMessageBox::critical(NULL, tr("Flight Plan Upload Failed"), tr("Failed to upload the flight plan !")); } } } @@ -90,8 +95,9 @@ void ModelUavoProxy::receiveFlightPlan() PathAction *action = PathAction::GetInstance(objMngr, 0); connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); - completionCount = 0; - completionSuccessCount = 0; + // we will start 3 update requests + completionCountdown = 3; + successCountdown = completionCountdown; flightPlan->requestUpdate(); waypoint->requestUpdateAll(); @@ -102,19 +108,18 @@ void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success) { obj->disconnect(this); - completionCount++; - if (success) { - completionSuccessCount++; - } + completionCountdown--; + successCountdown -= success ? 1 : 0; - if (completionCount == 3) { - qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (completionSuccessCount == 3); - if (completionSuccessCount == 3) { - objectsToModel(); - QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); + if (completionCountdown == 0) { + qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (successCountdown == 0); + if (successCountdown == 0) { + if (objectsToModel()) { + QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); + } } else { - QMessageBox::critical(NULL, tr("Flight Plan Download Failed !"), tr("Failed to download the flight plan !")); + QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Failed to download the flight plan !")); } } } @@ -129,7 +134,7 @@ void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success) // (compression consists in keeping only one instance of similar path actions) // // the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances -void ModelUavoProxy::modelToObjects() +bool ModelUavoProxy::modelToObjects() { qDebug() << "ModelUAVProxy::modelToObjects"; @@ -186,13 +191,13 @@ void ModelUavoProxy::modelToObjects() // update UAVObject waypoint->setData(waypointData); } - // Put "safe" values in unused waypoint objects + + // Put "safe" values in unused waypoint and path action objects if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) { for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) { // TODO } } - // Put "safe" values in unused path action objects if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) { for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) { // TODO @@ -205,10 +210,11 @@ void ModelUavoProxy::modelToObjects() flightPlanData.WaypointCount = waypointCount; flightPlanData.PathActionCount = actionCount; - // TODO - flightPlanData.Crc = 0; + flightPlanData.Crc = computeFlightPlanCrc(waypointCount, actionCount); flightPlan->setData(flightPlanData); + + return true; } Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { @@ -287,7 +293,7 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD return NULL; } -void ModelUavoProxy::objectsToModel() +bool ModelUavoProxy::objectsToModel() { // build model from uav objects // the list of objects can end with "garbage" instances due to previous flightpath @@ -297,9 +303,21 @@ void ModelUavoProxy::objectsToModel() FlightPlan::DataFields flightPlanData = flightPlan->getData(); int waypointCount = flightPlanData.WaypointCount; + int actionCount = flightPlanData.PathActionCount; - // TODO consistency checks - // objMngr->getNumInstances(Waypoint::OBJID); + // consistency check + if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) { + QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Flight plan way point count error !")); + return false; + } + if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) { + QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Flight plan path action count error !")); + return false; + } + if (flightPlanData.Crc != computeFlightPlanCrc(waypointCount, actionCount)) { + QMessageBox::critical(NULL, tr("Flight Plan Upload Failed"), tr("Flight plan CRC error !")); + return false; + } int rowCount = myModel->rowCount(); if (waypointCount < rowCount) { @@ -328,6 +346,7 @@ void ModelUavoProxy::objectsToModel() PathAction::DataFields actionData = action->getData(); pathActionToModel(i, actionData); } + return true; } void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { @@ -436,3 +455,17 @@ void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { index = myModel->index(i, flightDataModel::MODE_PARAMS3); myModel->setData(index, data.ModeParameters[3]); } + +quint8 ModelUavoProxy::computeFlightPlanCrc(int waypointCount, int actionCount) { + quint8 crc = 0; + for (int i = 0; i < waypointCount; ++i) { + Waypoint* waypoint = Waypoint::GetInstance(objMngr, i); + crc = waypoint->updateCRC(crc); + } + for (int i = 0; i < actionCount; ++i) { + PathAction* action = PathAction::GetInstance(objMngr, i); + crc = action->updateCRC(crc); + } + return crc; +} + diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 23e81557b..b1946c1dc 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -49,11 +49,11 @@ private: UAVObjectManager *objMngr; flightDataModel *myModel; - uint completionCount; - uint completionSuccessCount; + uint completionCountdown; + uint successCountdown; - void modelToObjects(); - void objectsToModel(); + bool modelToObjects(); + bool objectsToModel(); Waypoint *createWaypoint(int index, Waypoint *newWaypoint); PathAction *createPathAction(int index, PathAction *newAction); @@ -66,10 +66,11 @@ private: void waypointToModel(int i, Waypoint::DataFields &data); void pathActionToModel(int i, PathAction::DataFields &data); + quint8 computeFlightPlanCrc(int waypointCount, int actionCount); + private slots: void flightPlanElementSent(UAVObject *, bool success); void flightPlanElementReceived(UAVObject *, bool success); - }; #endif // MODELUAVOPROXY_H From 2ce0cb79099d09d6b6774fc65c64ef1c6496f56f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 12 Jan 2014 20:13:00 +0100 Subject: [PATCH 098/113] reverted changes to fixedwingpathfollower - wrong module --- .../fixedwingpathfollower.c | 57 +------------------ 1 file changed, 2 insertions(+), 55 deletions(-) diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 3226d80bd..5858bf5e0 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -45,13 +45,7 @@ #include -#include -#include - #include "hwsettings.h" -#include "flightplan.h" -#include "waypoint.h" -#include "pathaction.h" #include "attitudestate.h" #include "pathdesired.h" // object that will be updated by the module #include "positionstate.h" @@ -68,6 +62,7 @@ #include "velocitydesired.h" #include "velocitystate.h" #include "taskinfo.h" +#include #include "paths.h" #include "CoordinateConversions.h" @@ -85,7 +80,6 @@ static PathStatusData pathStatus; static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; // Private functions -static bool checkFlightPlan(); static void pathfollowerTask(void *parameters); static void SettingsUpdatedCb(UAVObjEvent *ev); static void updatePathVelocity(); @@ -169,7 +163,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - // 3. FlightPlan must be valid SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && @@ -179,15 +172,11 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) vTaskDelay(1000); continue; } - if (!checkFlightPlan()) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); @@ -253,48 +242,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) } } -static bool checkFlightPlan() -{ - uint16_t i; - uint16_t waypointCount; - uint16_t actionCount; - uint8_t flightCrc; - FlightPlanData flightPlan; - - FlightPlanGet(&flightPlan); - - waypointCount = flightPlan.WaypointCount; - actionCount = flightPlan.PathActionCount; - - // check count consistency - if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { - PIOS_DEBUGLOG_Printf("FlighPlan : waypoint count error!"); - return false; - } - if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { - PIOS_DEBUGLOG_Printf("FlighPlan : path action count error!"); - return false; - } - - // check CRC - flightCrc = 0; - for(i = 0; i < waypointCount; i++) { - flightCrc = UAVObjUpdateCRC(WaypointHandle(), i, flightCrc); - } - for(i = 0; i < actionCount; i++) { - flightCrc = UAVObjUpdateCRC(PathActionHandle(), i, flightCrc); - } - if (flightCrc != flightPlan.Crc) { - PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", flightCrc, flightPlan.Crc); - return false; - } - - // everything ok (hopefully...) - PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", flightCrc, flightPlan.Crc); - - return true; -} - /** * Compute desired velocity from the current position and path * From b8118f51e709e4b845ec4bbe461738b4c51c4547 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 11 Jan 2014 20:03:36 +0100 Subject: [PATCH 099/113] OP-1156 Made PathPlanner work with delayed callbacks in "navigation" callback task Conflicts: flight/modules/PathPlanner/pathplanner.c --- flight/modules/PathPlanner/pathplanner.c | 225 ++++++++++++----------- 1 file changed, 116 insertions(+), 109 deletions(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index eb43352c2..5f5a3b1d9 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -47,15 +47,17 @@ // Private constants #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define TASK_PRIORITY CALLBACK_TASK_NAVIGATION #define MAX_QUEUE_SIZE 2 -#define PATH_PLANNER_UPDATE_RATE_MS 20 +#define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well // Private types // Private functions -static void pathPlannerTask(void *parameters); -static void updatePathDesired(UAVObjEvent *ev); +static void pathPlannerTask(); +static void commandUpdated(UAVObjEvent *ev); +static void statusUpdated(UAVObjEvent *ev); +static void updatePathDesired(); static void setWaypoint(uint16_t num); static uint8_t pathConditionCheck(); @@ -72,7 +74,8 @@ static uint8_t conditionImmediate(); // Private variables -static xTaskHandle taskHandle; +static DelayedCallbackInfo *pathPlannerHandle; +static DelayedCallbackInfo *pathDesiredUpdaterHandle; static WaypointActiveData waypointActive; static WaypointData waypoint; static PathActionData pathAction; @@ -84,11 +87,14 @@ static bool pathplanner_active = false; */ int32_t PathPlannerStart() { - taskHandle = NULL; + // when the active waypoint changes, update pathDesired + WaypointConnectCallback(commandUpdated); + WaypointActiveConnectCallback(commandUpdated); + PathActionConnectCallback(commandUpdated); + PathStatusConnectCallback(statusUpdated); - // Start VM thread - xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle); + // Start main task callback + DelayedCallbackDispatch(pathPlannerHandle); return 0; } @@ -98,8 +104,6 @@ int32_t PathPlannerStart() */ int32_t PathPlannerInitialize() { - taskHandle = NULL; - FlightPlanInitialize(); PathActionInitialize(); PathStatusInitialize(); @@ -110,6 +114,9 @@ int32_t PathPlannerInitialize() WaypointInitialize(); WaypointActiveInitialize(); + pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES); + pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES); + return 0; } @@ -118,129 +125,129 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart); /** * Module task */ -static void pathPlannerTask(__attribute__((unused)) void *parameters) +static void pathPlannerTask() { - // when the active waypoint changes, update pathDesired - WaypointConnectCallback(updatePathDesired); - WaypointActiveConnectCallback(updatePathDesired); - PathActionConnectCallback(updatePathDesired); + DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + + bool endCondition = false; FlightStatusData flightStatus; - PathDesiredData pathDesired; + FlightStatusGet(&flightStatus); + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { + pathplanner_active = false; + + return; + } + + WaypointActiveGet(&waypointActive); + + if (pathplanner_active == false) { + pathplanner_active = true; + + // This triggers callback to update variable + waypointActive.Index = 0; + WaypointActiveSet(&waypointActive); + + return; + } + + WaypointInstGet(waypointActive.Index, &waypoint); + PathActionInstGet(waypoint.Action, &pathAction); PathStatusData pathStatus; + PathStatusGet(&pathStatus); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); - // Main thread loop - bool endCondition = false; - while (1) { - vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS); + // delay next step until path follower has acknowledged the path mode + if (pathStatus.UID != pathDesired.UID) { + return; + } - FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { - pathplanner_active = false; - continue; + // negative destinations DISABLE this feature + if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) { + setWaypoint(pathAction.ErrorDestination); + return; + } + + // check if condition has been met + endCondition = pathConditionCheck(); + // decide what to do + switch (pathAction.Command) { + case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: + endCondition = !endCondition; + case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: + if (endCondition) { + setWaypoint(waypointActive.Index + 1); } - - WaypointActiveGet(&waypointActive); - - if (pathplanner_active == false) { - pathplanner_active = true; - - // This triggers callback to update variable - waypointActive.Index = 0; - WaypointActiveSet(&waypointActive); - - continue; - } - - WaypointInstGet(waypointActive.Index, &waypoint); - PathActionInstGet(waypoint.Action, &pathAction); - PathStatusGet(&pathStatus); - PathDesiredGet(&pathDesired); - - // delay next step until path follower has acknowledged the path mode - if (pathStatus.UID != pathDesired.UID) { - continue; - } - - // negative destinations DISABLE this feature - if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) { - setWaypoint(pathAction.ErrorDestination); - continue; - } - - // check if condition has been met - endCondition = pathConditionCheck(); - - // decide what to do - switch (pathAction.Command) { - case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: - endCondition = !endCondition; - case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: - if (endCondition) { - setWaypoint(waypointActive.Index + 1); - } - break; - case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: - endCondition = !endCondition; - case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: - if (endCondition) { - if (pathAction.JumpDestination < 0) { - // waypoint ids <0 code relative jumps - setWaypoint(waypointActive.Index - pathAction.JumpDestination); - } else { - setWaypoint(pathAction.JumpDestination); - } - } - break; - case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: - if (endCondition) { - if (pathAction.JumpDestination < 0) { - // waypoint ids <0 code relative jumps - setWaypoint(waypointActive.Index - pathAction.JumpDestination); - } else { - setWaypoint(pathAction.JumpDestination); - } + break; + case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: + endCondition = !endCondition; + case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: + if (endCondition) { + if (pathAction.JumpDestination < 0) { + // waypoint ids <0 code relative jumps + setWaypoint(waypointActive.Index - pathAction.JumpDestination); } else { - setWaypoint(waypointActive.Index + 1); + setWaypoint(pathAction.JumpDestination); } - break; } + break; + case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: + if (endCondition) { + if (pathAction.JumpDestination < 0) { + // waypoint ids <0 code relative jumps + setWaypoint(waypointActive.Index - pathAction.JumpDestination); + } else { + setWaypoint(pathAction.JumpDestination); + } + } else { + setWaypoint(waypointActive.Index + 1); + } + break; } } +// callback function when status changed, issue execution of state machine +void commandUpdated(__attribute__((unused)) UAVObjEvent *ev) +{ + DelayedCallbackDispatch(pathDesiredUpdaterHandle); +} + // callback function when waypoints changed in any way, update pathDesired -void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) +void statusUpdated(__attribute__((unused)) UAVObjEvent *ev) +{ + DelayedCallbackDispatch(pathPlannerHandle); +} + + +// callback function when waypoints changed in any way, update pathDesired +void updatePathDesired() { // only ever touch pathDesired if pathplanner is enabled if (!pathplanner_active) { return; } - // use local variables, dont use stack since this is huge and a callback, - // dont use the globals because we cant use mutexes here - static WaypointActiveData waypointActiveData; - static PathActionData pathActionData; - static WaypointData waypointData; - static PathDesiredData pathDesired; + PathDesiredData pathDesired; // find out current waypoint - WaypointActiveGet(&waypointActiveData); + WaypointActiveGet(&waypointActive); - WaypointInstGet(waypointActiveData.Index, &waypointData); - PathActionInstGet(waypointData.Action, &pathActionData); + WaypointInstGet(waypointActive.Index, &waypoint); + PathActionInstGet(waypoint.Action, &pathAction); - pathDesired.End.North = waypointData.Position.North; - pathDesired.End.East = waypointData.Position.East; - pathDesired.End.Down = waypointData.Position.Down; - pathDesired.EndingVelocity = waypointData.Velocity; - pathDesired.Mode = pathActionData.Mode; - pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; - pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1]; - pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2]; - pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3]; - pathDesired.UID = waypointActiveData.Index; + pathDesired.End.North = waypoint.Position.North; + pathDesired.End.East = waypoint.Position.East; + pathDesired.End.Down = waypoint.Position.Down; + pathDesired.EndingVelocity = waypoint.Velocity; + pathDesired.Mode = pathAction.Mode; + pathDesired.ModeParameters[0] = pathAction.ModeParameters[0]; + pathDesired.ModeParameters[1] = pathAction.ModeParameters[1]; + pathDesired.ModeParameters[2] = pathAction.ModeParameters[2]; + pathDesired.ModeParameters[3] = pathAction.ModeParameters[3]; + pathDesired.UID = waypointActive.Index; - if (waypointActiveData.Index == 0) { + if (waypointActive.Index == 0) { PositionStateData positionState; PositionStateGet(&positionState); // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) From cb9e76b0069acc054d8318d219bdb04fd4a40fc6 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 12 Jan 2014 21:04:31 +0100 Subject: [PATCH 100/113] OP-1122 add flightplan safety check to pathplanner - failsafe rth when flightmode is set to pathplan but no valid plan is uploaded --- flight/modules/PathPlanner/pathplanner.c | 89 ++++++++++++++++++++- shared/uavobjectdefinition/systemalarms.xml | 1 + 2 files changed, 87 insertions(+), 3 deletions(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 5f5a3b1d9..0572f86ed 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -41,7 +41,7 @@ #include "velocitystate.h" #include "waypoint.h" #include "waypointactive.h" -#include "taskinfo.h" +#include "manualcontrolsettings.h" #include #include "paths.h" @@ -60,6 +60,7 @@ static void statusUpdated(UAVObjEvent *ev); static void updatePathDesired(); static void setWaypoint(uint16_t num); +static uint8_t checkFlightPlan(); static uint8_t pathConditionCheck(); static uint8_t conditionNone(); static uint8_t conditionTimeOut(); @@ -131,6 +132,15 @@ static void pathPlannerTask() bool endCondition = false; + // check flight plan validity early to raise alarm + // even if not in guided mode + uint8_t validFlightPlan = checkFlightPlan(); + if (!validFlightPlan) { + AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN); + } + FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { @@ -139,6 +149,38 @@ static void pathPlannerTask() return; } + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + + static uint8_t failsafeRTHset = 0; + if (!validFlightPlan) { + // At this point the craft is in PathPlanner mode, so pilot has no manual control capability. + // Failsafe: behave as if in return to base mode + // what to do in this case is debatable, it might be better to just make a forced disarming but rth has a higher chance of survival when in flight. + pathplanner_active = false; + + if (!failsafeRTHset) { + failsafeRTHset = 1; + // copy pasta: same calculation as in manualcontrol, set return to home coordinates + PositionStateData positionState; + PositionStateGet(&positionState); + ManualControlSettingsData settings; + ManualControlSettingsGet(&settings); + + pathDesired.Start.North = 0; + pathDesired.Start.East = 0; + pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.End.North = 0; + pathDesired.End.East = 0; + pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } + } + failsafeRTHset = 0; + WaypointActiveGet(&waypointActive); if (pathplanner_active == false) { @@ -155,8 +197,6 @@ static void pathPlannerTask() PathActionInstGet(waypoint.Action, &pathAction); PathStatusData pathStatus; PathStatusGet(&pathStatus); - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); // delay next step until path follower has acknowledged the path mode if (pathStatus.UID != pathDesired.UID) { @@ -207,6 +247,49 @@ static void pathPlannerTask() } } +// safety check for path plan integrity +static uint8_t checkFlightPlan() +{ + uint16_t i; + uint16_t waypointCount; + uint16_t actionCount; + uint8_t flightCrc; + FlightPlanData flightPlan; + + FlightPlanGet(&flightPlan); + + waypointCount = flightPlan.WaypointCount; + actionCount = flightPlan.PathActionCount; + + // check count consistency + if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { + PIOS_DEBUGLOG_Printf("FlighPlan : waypoint count error!"); + return false; + } + if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { + PIOS_DEBUGLOG_Printf("FlighPlan : path action count error!"); + return false; + } + + // check CRC + flightCrc = 0; + for (i = 0; i < waypointCount; i++) { + flightCrc = UAVObjUpdateCRC(WaypointHandle(), i, flightCrc); + } + for (i = 0; i < actionCount; i++) { + flightCrc = UAVObjUpdateCRC(PathActionHandle(), i, flightCrc); + } + if (flightCrc != flightPlan.Crc) { + PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", flightCrc, flightPlan.Crc); + return false; + } + + // everything ok (hopefully...) + PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", flightCrc, flightPlan.Crc); + + return true; +} + // callback function when status changed, issue execution of state machine void commandUpdated(__attribute__((unused)) UAVObjEvent *ev) { diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index 34c353d64..f3763ad53 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -16,6 +16,7 @@ Sensors Stabilization Guidance + PathPlan Battery FlightTime I2C From 3d5e27f5f2256cf60d56c3311d5faddf29d2988b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 12 Jan 2014 21:06:41 +0100 Subject: [PATCH 101/113] OP-1122 fixed simposix so it will compile with new objects --- flight/targets/boards/simposix/firmware/Makefile | 2 +- flight/targets/boards/simposix/firmware/UAVObjects.inc | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index c9d719334..0cb868cf7 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -68,7 +68,7 @@ UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight # optional component libraries include $(PIOS)/common/libraries/FreeRTOS/library.mk -include $(FLIGHTLIB)/PyMite/pymite.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk # List C source files here. (C dependencies are automatically generated.) # use file-extension c for "c-only"-files diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index be99363d3..59feac088 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -49,6 +49,7 @@ UAVOBJSRCFILENAMES += debuglogentry UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplan UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus From 76ee48bc441565850b45460be73f3ac5168674dd Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 12 Jan 2014 21:37:40 +0100 Subject: [PATCH 102/113] OP-1122 safer sanity checks and arming prevention in case of invalid flight plan --- flight/libraries/sanitycheck.c | 13 ++++++++++--- flight/modules/PathPlanner/pathplanner.c | 22 ++++++++++++++++------ 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 51442f48e..7efaa057b 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -35,6 +35,7 @@ // UAVOs #include #include +#include #include /**************************** @@ -151,9 +152,15 @@ int32_t configuration_check() case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports PathPlanner - severity = SYSTEMALARMS_ALARM_ERROR; + } else { + // Revo supports PathPlanner and that must be OK or we are not sane + // PathPlan alarm is uninitialized if not running + // PathPlan alarm is warning or error if the flightplan is invalid + SystemAlarmsAlarmData alarms; + SystemAlarmsAlarmGet(&alarms); + if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) { + severity = SYSTEMALARMS_ALARM_ERROR; + } } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 0572f86ed..25bc3d97e 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -135,16 +135,20 @@ static void pathPlannerTask() // check flight plan validity early to raise alarm // even if not in guided mode uint8_t validFlightPlan = checkFlightPlan(); - if (!validFlightPlan) { - AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN); - } FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { pathplanner_active = false; + if (!validFlightPlan) { + // unverified flight plans are only a warning while we are not in pathplanner mode + // so it does not prevent arming. However manualcontrols safety check + // shall test for this warning when pathplan is on the flight mode selector + // thus a valid flight plan is a prerequirement for arming + AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN); + } return; } @@ -156,7 +160,9 @@ static void pathPlannerTask() if (!validFlightPlan) { // At this point the craft is in PathPlanner mode, so pilot has no manual control capability. // Failsafe: behave as if in return to base mode - // what to do in this case is debatable, it might be better to just make a forced disarming but rth has a higher chance of survival when in flight. + // what to do in this case is debatable, it might be better to just + // make a forced disarming but rth has a higher chance of survival when + // in flight. pathplanner_active = false; if (!failsafeRTHset) { @@ -178,8 +184,12 @@ static void pathPlannerTask() pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } + AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR); + + return; } failsafeRTHset = 0; + AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN); WaypointActiveGet(&waypointActive); From 07d5c8e4d28dd801209994a719915c5aa2a3ee94 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 14 Jan 2014 00:26:27 +0100 Subject: [PATCH 103/113] OP-1022 fixed the config plugin widget to represent new simpler settings layout --- flight/modules/ManualControl/manualcontrol.c | 2 +- .../src/plugins/config/stabilization.ui | 42 +++++++++++-------- .../altitudeholdsettings.xml | 2 +- 3 files changed, 26 insertions(+), 20 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index dbc7ecc03..7dcfb32ea 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -850,7 +850,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; // this is the max speed in m/s at the extents of throttle - uint8_t throttleRate; + float throttleRate; uint8_t throttleExp; static uint8_t flightMode; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 61352bd2f..e4a520311 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -28291,7 +28291,7 @@ border-radius: 5; - Integral + Velocity Proportional Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28316,7 +28316,7 @@ border-radius: 5; - Proportional + Altitude Proportional Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28347,7 +28347,7 @@ border-radius: 5; - Derivative + Velocity Integral Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28371,8 +28371,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kp - scale:0.001 + fieldname:AltitudePI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -28396,8 +28397,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Ki - scale:0.001 + fieldname:VelocityPI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -28407,7 +28409,7 @@ border-radius: 5; - 100 + 1000 50 @@ -28421,8 +28423,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kd - scale:0.001 + fieldname:VelocityPI + element:Ki + scale:0.00001 haslimits:yes buttongroup:99 @@ -28988,7 +28991,7 @@ color: rgb(255, 255, 255); border-radius: 5; - Altitude + Control Coefficients Qt::AlignCenter @@ -29033,8 +29036,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kp - scale:0.001 + fieldname:AltitudePI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -29079,8 +29083,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Ki - scale:0.001 + fieldname:VelocityPI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -29117,7 +29122,7 @@ border-radius: 5; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - 100 + 1000 51 @@ -29125,8 +29130,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kd - scale:0.001 + fieldname:VelocityPI + element:Ki + scale:0.00001 haslimits:yes buttongroup:99 diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 8e88100f1..174468344 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -5,7 +5,7 @@ - + From 7f2c24db2b4f7240df245491bf0e27201c2b9a5f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 13 Jan 2014 22:21:34 +0100 Subject: [PATCH 104/113] OP-1122 OP-1158 - renamed uavobject FlightPlan to PathPlan +review OPReview-609 --- flight/modules/PathPlanner/pathplanner.c | 48 ++++++------ .../boards/revolution/firmware/UAVObjects.inc | 2 +- .../boards/revoproto/firmware/UAVObjects.inc | 2 +- .../src/plugins/opmap/modeluavoproxy.cpp | 74 +++++++++---------- .../src/plugins/opmap/modeluavoproxy.h | 12 +-- .../src/plugins/opmap/opmapgadgetwidget.cpp | 4 +- .../src/plugins/uavobjects/uavobjects.pro | 4 +- .../{flightplan.xml => pathplan.xml} | 2 +- 8 files changed, 74 insertions(+), 74 deletions(-) rename shared/uavobjectdefinition/{flightplan.xml => pathplan.xml} (86%) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 25bc3d97e..de476b06a 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -31,7 +31,7 @@ #include "openpilot.h" -#include "flightplan.h" +#include "pathplan.h" #include "flightstatus.h" #include "airspeedstate.h" #include "pathaction.h" @@ -60,7 +60,7 @@ static void statusUpdated(UAVObjEvent *ev); static void updatePathDesired(); static void setWaypoint(uint16_t num); -static uint8_t checkFlightPlan(); +static uint8_t checkPathPlan(); static uint8_t pathConditionCheck(); static uint8_t conditionNone(); static uint8_t conditionTimeOut(); @@ -105,7 +105,7 @@ int32_t PathPlannerStart() */ int32_t PathPlannerInitialize() { - FlightPlanInitialize(); + PathPlanInitialize(); PathActionInitialize(); PathStatusInitialize(); PathDesiredInitialize(); @@ -132,16 +132,16 @@ static void pathPlannerTask() bool endCondition = false; - // check flight plan validity early to raise alarm + // check path plan validity early to raise alarm // even if not in guided mode - uint8_t validFlightPlan = checkFlightPlan(); + uint8_t validPathPlan = checkPathPlan(); FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { pathplanner_active = false; - if (!validFlightPlan) { - // unverified flight plans are only a warning while we are not in pathplanner mode + if (!validPathPlan) { + // unverified path plans are only a warning while we are not in pathplanner mode // so it does not prevent arming. However manualcontrols safety check // shall test for this warning when pathplan is on the flight mode selector // thus a valid flight plan is a prerequirement for arming @@ -157,7 +157,7 @@ static void pathPlannerTask() PathDesiredGet(&pathDesired); static uint8_t failsafeRTHset = 0; - if (!validFlightPlan) { + if (!validPathPlan) { // At this point the craft is in PathPlanner mode, so pilot has no manual control capability. // Failsafe: behave as if in return to base mode // what to do in this case is debatable, it might be better to just @@ -258,18 +258,18 @@ static void pathPlannerTask() } // safety check for path plan integrity -static uint8_t checkFlightPlan() +static uint8_t checkPathPlan() { uint16_t i; uint16_t waypointCount; uint16_t actionCount; - uint8_t flightCrc; - FlightPlanData flightPlan; + uint8_t pathCrc; + PathPlanData pathPlan; - FlightPlanGet(&flightPlan); + PathPlanGet(&pathPlan); - waypointCount = flightPlan.WaypointCount; - actionCount = flightPlan.PathActionCount; + waypointCount = pathPlan.WaypointCount; + actionCount = pathPlan.PathActionCount; // check count consistency if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { @@ -282,20 +282,20 @@ static uint8_t checkFlightPlan() } // check CRC - flightCrc = 0; + pathCrc = 0; for (i = 0; i < waypointCount; i++) { - flightCrc = UAVObjUpdateCRC(WaypointHandle(), i, flightCrc); + pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc); } for (i = 0; i < actionCount; i++) { - flightCrc = UAVObjUpdateCRC(PathActionHandle(), i, flightCrc); + pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc); } - if (flightCrc != flightPlan.Crc) { - PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", flightCrc, flightPlan.Crc); + if (pathCrc != pathPlan.Crc) { + PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc); return false; } // everything ok (hopefully...) - PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", flightCrc, flightPlan.Crc); + PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", pathCrc, pathPlan.Crc); return true; } @@ -368,12 +368,12 @@ void updatePathDesired() // helper function to go to a specific waypoint static void setWaypoint(uint16_t num) { - FlightPlanData flightPlan; + PathPlanData pathPlan; - FlightPlanGet(&flightPlan); + PathPlanGet(&pathPlan); - // here it is assumed that the flight plan has been validated (waypoint count is consistent) - if (num >= flightPlan.WaypointCount) { + // here it is assumed that the path plan has been validated (waypoint count is consistent) + if (num >= pathPlan.WaypointCount) { // path plans wrap around num = 0; } diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 337a2123a..043f1f18c 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -43,7 +43,6 @@ UAVOBJSRCFILENAMES += debuglogentry UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate -UAVOBJSRCFILENAMES += flightplan UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus @@ -71,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index d0480fa26..01d8d9a89 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -44,7 +44,6 @@ UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate -UAVOBJSRCFILENAMES += flightplan UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus @@ -71,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index dfa6b4818..8b31c1d65 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -41,32 +41,32 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec successCountdown = 0; } -void ModelUavoProxy::sendFlightPlan() +void ModelUavoProxy::sendPathPlan() { modelToObjects(); - FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr, 0); - connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), - this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); + PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0); + connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), + this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), - this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); + this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); PathAction *action = PathAction::GetInstance(objMngr, 0); connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), - this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); + this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection); // we will start 3 update all completionCountdown = 3; successCountdown = completionCountdown; - flightPlan->updated(); + pathPlan->updated(); waypoint->updatedAll(); action->updatedAll(); } -void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success) +void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success) { obj->disconnect(this); @@ -74,37 +74,37 @@ void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success) successCountdown -= success ? 1 : 0; if (completionCountdown == 0) { - qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (successCountdown == 0); + qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0); if (successCountdown == 0) { - QMessageBox::information(NULL, tr("Flight Plan Upload Successful"), tr("Flight plan upload was successful.")); + QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful.")); } else { - QMessageBox::critical(NULL, tr("Flight Plan Upload Failed"), tr("Failed to upload the flight plan !")); + QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !")); } } } -void ModelUavoProxy::receiveFlightPlan() +void ModelUavoProxy::receivePathPlan() { - FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr, 0); - connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); + PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0); + connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool))); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); - connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool))); PathAction *action = PathAction::GetInstance(objMngr, 0); - connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool))); // we will start 3 update requests completionCountdown = 3; successCountdown = completionCountdown; - flightPlan->requestUpdate(); + pathPlan->requestUpdate(); waypoint->requestUpdateAll(); action->requestUpdateAll(); } -void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success) +void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success) { obj->disconnect(this); @@ -112,14 +112,14 @@ void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success) successCountdown -= success ? 1 : 0; if (completionCountdown == 0) { - qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (successCountdown == 0); + qDebug() << "ModelUavoProxy::pathPlanReceived - completed" << (successCountdown == 0); if (successCountdown == 0) { if (objectsToModel()) { - QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); + QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful.")); } } else { - QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Failed to download the flight plan !")); + QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !")); } } } @@ -204,15 +204,15 @@ bool ModelUavoProxy::modelToObjects() } } - // Update FlightPlan - FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr); - FlightPlan::DataFields flightPlanData = flightPlan->getData(); + // Update PathPlan + PathPlan *pathPlan = PathPlan::GetInstance(objMngr); + PathPlan::DataFields pathPlanData = pathPlan->getData(); - flightPlanData.WaypointCount = waypointCount; - flightPlanData.PathActionCount = actionCount; - flightPlanData.Crc = computeFlightPlanCrc(waypointCount, actionCount); + pathPlanData.WaypointCount = waypointCount; + pathPlanData.PathActionCount = actionCount; + pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount); - flightPlan->setData(flightPlanData); + pathPlan->setData(pathPlanData); return true; } @@ -299,23 +299,23 @@ bool ModelUavoProxy::objectsToModel() // the list of objects can end with "garbage" instances due to previous flightpath // they need to be ignored - FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr); - FlightPlan::DataFields flightPlanData = flightPlan->getData(); + PathPlan *pathPlan = PathPlan::GetInstance(objMngr); + PathPlan::DataFields pathPlanData = pathPlan->getData(); - int waypointCount = flightPlanData.WaypointCount; - int actionCount = flightPlanData.PathActionCount; + int waypointCount = pathPlanData.WaypointCount; + int actionCount = pathPlanData.PathActionCount; // consistency check if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) { - QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Flight plan way point count error !")); + QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan way point count error !")); return false; } if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) { - QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Flight plan path action count error !")); + QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan path action count error !")); return false; } - if (flightPlanData.Crc != computeFlightPlanCrc(waypointCount, actionCount)) { - QMessageBox::critical(NULL, tr("Flight Plan Upload Failed"), tr("Flight plan CRC error !")); + if (pathPlanData.Crc != computePathPlanCrc(waypointCount, actionCount)) { + QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Path plan CRC error !")); return false; } @@ -456,7 +456,7 @@ void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { myModel->setData(index, data.ModeParameters[3]); } -quint8 ModelUavoProxy::computeFlightPlanCrc(int waypointCount, int actionCount) { +quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount) { quint8 crc = 0; for (int i = 0; i < waypointCount; ++i) { Waypoint* waypoint = Waypoint::GetInstance(objMngr, i); diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index b1946c1dc..d62d67746 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -29,7 +29,7 @@ #include "flightdatamodel.h" -#include "flightplan.h" +#include "pathplan.h" #include "pathaction.h" #include "waypoint.h" @@ -42,8 +42,8 @@ public: explicit ModelUavoProxy(QObject *parent, flightDataModel *model); public slots: - void sendFlightPlan(); - void receiveFlightPlan(); + void sendPathPlan(); + void receivePathPlan(); private: UAVObjectManager *objMngr; @@ -66,11 +66,11 @@ private: void waypointToModel(int i, Waypoint::DataFields &data); void pathActionToModel(int i, PathAction::DataFields &data); - quint8 computeFlightPlanCrc(int waypointCount, int actionCount); + quint8 computePathPlanCrc(int waypointCount, int actionCount); private slots: - void flightPlanElementSent(UAVObject *, bool success); - void flightPlanElementReceived(UAVObject *, bool success); + void pathPlanElementSent(UAVObject *, bool success); + void pathPlanElementReceived(UAVObject *, bool success); }; #endif // MODELUAVOPROXY_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 43e7ac02b..443957fbc 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -228,8 +228,8 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) UAVProxy = new ModelUavoProxy(this, model); // sending and receiving is asynchronous // TODO : buttons should be disabled while a send or receive is in progress - connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendFlightPlan())); - connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receiveFlightPlan())); + connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendPathPlan())); + connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receivePathPlan())); #endif magicWayPoint = m_map->magicWPCreate(); magicWayPoint->setVisible(false); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 4e0153d11..3bffd84ea 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -77,6 +77,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/gpssettings.h \ $$UAVOBJECT_SYNTHETICS/pathaction.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ + $$UAVOBJECT_SYNTHETICS/pathplan.h \ $$UAVOBJECT_SYNTHETICS/pathstatus.h \ $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \ $$UAVOBJECT_SYNTHETICS/positionstate.h \ @@ -97,7 +98,6 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.h \ $$UAVOBJECT_SYNTHETICS/taskinfo.h \ - $$UAVOBJECT_SYNTHETICS/flightplan.h \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.h \ $$UAVOBJECT_SYNTHETICS/flightplansettings.h \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \ @@ -171,6 +171,7 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/gpssettings.cpp \ $$UAVOBJECT_SYNTHETICS/pathaction.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ + $$UAVOBJECT_SYNTHETICS/pathplan.cpp \ $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \ $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \ $$UAVOBJECT_SYNTHETICS/positionstate.cpp \ @@ -191,7 +192,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.cpp \ $$UAVOBJECT_SYNTHETICS/taskinfo.cpp \ - $$UAVOBJECT_SYNTHETICS/flightplan.cpp \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.cpp \ $$UAVOBJECT_SYNTHETICS/flightplansettings.cpp \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \ diff --git a/shared/uavobjectdefinition/flightplan.xml b/shared/uavobjectdefinition/pathplan.xml similarity index 86% rename from shared/uavobjectdefinition/flightplan.xml rename to shared/uavobjectdefinition/pathplan.xml index e8c7b61c9..4db042a85 100644 --- a/shared/uavobjectdefinition/flightplan.xml +++ b/shared/uavobjectdefinition/pathplan.xml @@ -1,5 +1,5 @@ - + Flight plan informations From 96588ab13e321287ed5e4be80386943ec6f04161 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 13 Jan 2014 23:15:04 +0100 Subject: [PATCH 105/113] OP-1122 OP-1145 fixed logging helper as per review +review OPReview-604 --- flight/uavtalk/uavtalk.c | 9 +++++++-- ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp | 7 ++++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 001fac196..f4909aca7 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -36,9 +36,14 @@ #if defined UAV_DEBUGLOG && defined FLASH_FREERTOS #define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__) -#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == 0x5E5903CC) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } -#else +// uncomment and adapt the following lines to filter verbose logging to include specific object(s) only +//#include "flighttelemetrystats.h" +//#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } +#endif +#ifndef UAVT_DEBUGLOG_PRINTF #define UAVT_DEBUGLOG_PRINTF(...) +#endif +#ifndef UAVT_DEBUGLOG_CPRINTF #define UAVT_DEBUGLOG_CPRINTF(objId, ...) #endif diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 890922116..1c7a33322 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -34,7 +34,12 @@ #include #ifdef VERBOSE_UAVTALK -#define VERBOSE_FILTER(objId) if (objId == 0x173E3850 || objId == 0x99C63292) +// uncomment and adapt the following lines to filter verbose logging to include specific object(s) only +//#include "flighttelemetrystats.h" +//#define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID) +#endif +#ifndef VERBOSE_FILTER +#define VERBOSE_FILTER(objId) #endif #define SYNC_VAL 0x3C From c8cb666271e1966a1cb531327c3c24abd62a6a22 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 14 Jan 2014 00:29:52 +0100 Subject: [PATCH 106/113] OP-1122 fixed wrong Timeout legend in waypoint editor (was (ms) instead of (s)) +review OPReview-609 --- .../src/plugins/opmap/opmap_edit_waypoint_dialog.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp index 1c69638d7..709532222 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp @@ -176,7 +176,7 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets() ui->dsb_condParam2->setVisible(false); ui->dsb_condParam3->setVisible(false); ui->dsb_condParam4->setVisible(false); - ui->condParam1->setText("Timeout(ms)"); + ui->condParam1->setText("Timeout(s)"); break; case MapDataDelegate::ENDCONDITION_DISTANCETOTARGET: ui->condParam1->setVisible(true); From 8ccdb252d1aa0df9db24e59c25c991a77bbab9a0 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 14 Jan 2014 20:49:19 +0100 Subject: [PATCH 107/113] OP-1122 OP-1145 commented out PIOS_DEBUGLOG_Printf calls in pathplanner +review OPReview-609 --- flight/modules/PathPlanner/pathplanner.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index de476b06a..3e705d99e 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -257,7 +257,7 @@ static void pathPlannerTask() } } -// safety check for path plan integrity +// safety checks for path plan integrity static uint8_t checkPathPlan() { uint16_t i; @@ -273,11 +273,11 @@ static uint8_t checkPathPlan() // check count consistency if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { - PIOS_DEBUGLOG_Printf("FlighPlan : waypoint count error!"); + //PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!"); return false; } if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { - PIOS_DEBUGLOG_Printf("FlighPlan : path action count error!"); + //PIOS_DEBUGLOG_Printf("PathPlan : path action count error!"); return false; } @@ -290,12 +290,12 @@ static uint8_t checkPathPlan() pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc); } if (pathCrc != pathPlan.Crc) { - PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc); + // failed crc check + //PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc); return false; } - // everything ok (hopefully...) - PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", pathCrc, pathPlan.Crc); + // path plan passed checks return true; } From 1fca85784c2cde87f408e2f12700d1dc8b35b860 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 14 Jan 2014 21:00:44 +0100 Subject: [PATCH 108/113] OP-1122 OP-1145 added more path plan checks : - empty plan (i.e. no waypoints defined) - out of range waypoint and path action ids +review OPReview-609 --- flight/modules/PathPlanner/pathplanner.c | 28 ++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 3e705d99e..93e664b85 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -265,10 +265,16 @@ static uint8_t checkPathPlan() uint16_t actionCount; uint8_t pathCrc; PathPlanData pathPlan; + //WaypointData waypoint; // using global instead (?) + //PathActionData action; // using global instead (?) PathPlanGet(&pathPlan); waypointCount = pathPlan.WaypointCount; + if (waypointCount == 0) { + // an empty path plan is invalid + return false; + } actionCount = pathPlan.PathActionCount; // check count consistency @@ -295,6 +301,28 @@ static uint8_t checkPathPlan() return false; } + // waypoint consistency + for (i = 0; i < waypointCount; i++) { + WaypointInstGet(i, &waypoint); + if (waypoint.Action >= actionCount) { + // path action id is out of range + return false; + } + } + + // path action consistency + for (i = 0; i < actionCount; i++) { + PathActionInstGet(i, &pathAction); + if (pathAction.ErrorDestination >= waypointCount) { + // waypoint id is out of range + return false; + } + if (pathAction.JumpDestination >= waypointCount) { + // waypoint id is out of range + return false; + } + } + // path plan passed checks return true; From d43b220dc0aab09d0db32a3d2531a40f6bc1541d Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Tue, 14 Jan 2014 22:46:01 +0100 Subject: [PATCH 109/113] OP-1122 uncrustified gcs and flight --- flight/modules/PathPlanner/pathplanner.c | 13 +- .../modules/RadioComBridge/RadioComBridge.c | 56 ++++---- flight/modules/Telemetry/telemetry.c | 54 +++----- flight/uavobjects/uavobjectmanager.c | 2 +- flight/uavtalk/inc/uavtalk.h | 1 - flight/uavtalk/inc/uavtalk_priv.h | 14 +- flight/uavtalk/uavtalk.c | 49 +++---- ground/openpilotgcs/src/libs/utils/crc.h | 4 - .../src/plugins/notify/notifyplugin.cpp | 1 + .../src/plugins/opmap/flightdatamodel.cpp | 8 +- .../src/plugins/opmap/modeluavoproxy.cpp | 129 ++++++++++-------- .../src/plugins/opmap/modeluavoproxy.h | 2 +- .../src/plugins/uavobjects/uavobject.cpp | 20 +-- .../uavobjectutil/uavobjectutilmanager.cpp | 1 + .../uavobjectwidgetutils/configtaskwidget.cpp | 1 + .../src/plugins/uavtalk/telemetry.cpp | 33 +++-- .../src/plugins/uavtalk/telemetrymanager.cpp | 9 +- .../src/plugins/uavtalk/telemetrymanager.h | 3 +- .../src/plugins/uavtalk/telemetrymonitor.cpp | 16 +-- .../src/plugins/uavtalk/uavtalk.cpp | 62 ++++----- .../src/plugins/uavtalk/uavtalk.h | 20 +-- 21 files changed, 248 insertions(+), 250 deletions(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 93e664b85..6c548b6b7 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -265,8 +265,9 @@ static uint8_t checkPathPlan() uint16_t actionCount; uint8_t pathCrc; PathPlanData pathPlan; - //WaypointData waypoint; // using global instead (?) - //PathActionData action; // using global instead (?) + + // WaypointData waypoint; // using global instead (?) + // PathActionData action; // using global instead (?) PathPlanGet(&pathPlan); @@ -275,15 +276,15 @@ static uint8_t checkPathPlan() // an empty path plan is invalid return false; } - actionCount = pathPlan.PathActionCount; + actionCount = pathPlan.PathActionCount; // check count consistency if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { - //PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!"); + // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!"); return false; } if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { - //PIOS_DEBUGLOG_Printf("PathPlan : path action count error!"); + // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!"); return false; } @@ -297,7 +298,7 @@ static uint8_t checkPathPlan() } if (pathCrc != pathPlan.Crc) { // failed crc check - //PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc); + // PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc); return false; } diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index f913ab672..22341a771 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -1,5 +1,6 @@ /** ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module @@ -133,9 +134,9 @@ static int32_t RadioComBridgeStart(void) data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); + data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && + (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && + (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { @@ -236,20 +237,20 @@ static int32_t RadioComBridgeInitialize(void) RadioComBridgeStatsInitialize(); // Initialise UAVTalk - data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. - data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); - data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); // Initialize the statistics. - data->telemetryTxRetries = 0; - data->radioTxRetries = 0; + data->telemetryTxRetries = 0; + data->radioTxRetries = 0; - data->parseUAVTalk = true; - data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; - PIOS_COM_RADIO = PIOS_COM_RFM22B; + data->parseUAVTalk = true; + data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; + PIOS_COM_RADIO = PIOS_COM_RFM22B; return 0; } @@ -264,13 +265,14 @@ static void registerObject(UAVObjHandle obj) { // Setup object for periodic updates UAVObjEvent ev = { - .obj = obj, - .instId = UAVOBJ_ALL_INSTANCES, - .event = EV_UPDATED_PERIODIC, + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .event = EV_UPDATED_PERIODIC, }; // Get metadata UAVObjMetadata metadata; + UAVObjGetMetadata(obj, &metadata); EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod); @@ -298,23 +300,23 @@ static void updateRadioComBridgeStats() RadioComBridgeStatsGet(&radioComBridgeStats); // Update stats object - radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; - radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; - radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; + radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; + radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; + radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; - radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; - radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; + radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; + radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors; - radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; + radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; - radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; - radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; - radioComBridgeStats.RadioTxRetries += data->radioTxRetries; + radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; + radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; + radioComBridgeStats.RadioTxRetries += data->radioTxRetries; - radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; - radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; + radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; + radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors; - radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; + radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; // Update stats object data RadioComBridgeStatsSet(&radioComBridgeStats); diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 443112773..ef1ab9ea4 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -182,16 +182,6 @@ static void registerObject(UAVObjHandle obj) UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); } else { // Setup object for periodic updates -// UAVObjEvent ev = { -// .obj = obj, -// .instId = UAVOBJ_ALL_INSTANCES, -// .event = EV_UPDATED_PERIODIC, -// }; -// EventPeriodicQueueCreate(&ev, queue, 0); -// ev.event = EV_LOGGING_PERIODIC; -// EventPeriodicQueueCreate(&ev, queue, 0); - - // Setup object for telemetry updates updateObject(obj, EV_NONE); } } @@ -312,8 +302,8 @@ static void processObjEvent(UAVObjEvent *ev) retries = 0; success = -1; if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) - || ev->event == EV_UPDATED_MANUAL - || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { + || ev->event == EV_UPDATED_MANUAL + || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until ack is received or timeout @@ -357,8 +347,8 @@ static void processObjEvent(UAVObjEvent *ev) if (ev->obj) { updateMode = UAVObjGetLoggingUpdateMode(&metadata); if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) - || ev->event == EV_LOGGING_MANUAL - || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { + || ev->event == EV_LOGGING_MANUAL + || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { if (ev->instId == UAVOBJ_ALL_INSTANCES) { success = UAVObjGetNumInstances(ev->obj); for (retries = 0; retries < success; retries++) { @@ -572,33 +562,33 @@ static void updateTelemetryStats() // Update stats object if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.TxBytes += utalkStats.txBytes; - flightStats.TxFailures += txErrors; - flightStats.TxRetries += txRetries; + flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.TxBytes += utalkStats.txBytes; + flightStats.TxFailures += txErrors; + flightStats.TxRetries += txRetries; - flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.RxBytes += utalkStats.rxBytes; - flightStats.RxFailures += utalkStats.rxErrors; + flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.RxBytes += utalkStats.rxBytes; + flightStats.RxFailures += utalkStats.rxErrors; flightStats.RxSyncErrors += utalkStats.rxSyncErrors; - flightStats.RxCrcErrors += utalkStats.rxCrcErrors; + flightStats.RxCrcErrors += utalkStats.rxCrcErrors; } else { - flightStats.TxDataRate = 0; - flightStats.TxBytes = 0; - flightStats.TxFailures = 0; - flightStats.TxRetries = 0; + flightStats.TxDataRate = 0; + flightStats.TxBytes = 0; + flightStats.TxFailures = 0; + flightStats.TxRetries = 0; - flightStats.RxDataRate = 0; - flightStats.RxBytes = 0; - flightStats.RxFailures = 0; + flightStats.RxDataRate = 0; + flightStats.RxBytes = 0; + flightStats.RxFailures = 0; flightStats.RxSyncErrors = 0; - flightStats.RxCrcErrors = 0; + flightStats.RxCrcErrors = 0; } - txErrors = 0; + txErrors = 0; txRetries = 0; // Check for connection timeout - timeNow = xTaskGetTickCount() * portTICK_RATE_MS; + timeNow = xTaskGetTickCount() * portTICK_RATE_MS; if (utalkStats.rxObjects > 0) { timeOfLastObjectUpdate = timeNow; } diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index bf5d52a80..f5077c982 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -732,7 +732,7 @@ uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc) goto unlock_exit; } // Update crc - crc = PIOS_CRC_updateCRC(crc, (uint8_t *) InstanceData(instEntry), (int32_t) obj->instance_size); + crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->instance_size); } unlock_exit: diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index 9a346be3a..e54231735 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -44,7 +44,6 @@ typedef struct { uint32_t rxErrors; uint32_t rxSyncErrors; uint32_t rxCrcErrors; - } UAVTalkStats; typedef void *UAVTalkConnection; diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index c7cab8d9e..73354845a 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -34,10 +34,10 @@ // Private types and constants // min header : sync(1), type (1), size(2), object ID(4), instance ID(2) -#define UAVTALK_MIN_HEADER_LENGTH 10 +#define UAVTALK_MIN_HEADER_LENGTH 10 // max header : sync(1), type (1), size(2), object ID(4), instance ID(2), timestamp(2) -#define UAVTALK_MAX_HEADER_LENGTH 12 +#define UAVTALK_MAX_HEADER_LENGTH 12 #define UAVTALK_CHECKSUM_LENGTH 1 @@ -47,11 +47,11 @@ #define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH typedef struct { - uint8_t type; - uint16_t packet_size; - uint32_t objId; - uint16_t instId; - uint32_t length; + uint8_t type; + uint16_t packet_size; + uint32_t objId; + uint16_t instId; + uint32_t length; uint8_t timestampLength; uint8_t cs; uint16_t timestamp; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index f4909aca7..0f9dd3d0e 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -32,13 +32,13 @@ #include "openpilot.h" #include "uavtalk_priv.h" -//#define UAV_DEBUGLOG 1 +// #define UAV_DEBUGLOG 1 #if defined UAV_DEBUGLOG && defined FLASH_FREERTOS #define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__) // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only -//#include "flighttelemetrystats.h" -//#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } +// #include "flighttelemetrystats.h" +// #define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } #endif #ifndef UAVT_DEBUGLOG_PRINTF #define UAVT_DEBUGLOG_PRINTF(...) @@ -296,6 +296,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type { int32_t respReceived; int32_t ret = -1; + // Send object depending on if a response is needed if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) { // Get transaction lock (will block if a transaction is pending) @@ -304,7 +305,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); // expected response type connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK; - connection->respObjId = UAVObjGetID(obj); + connection->respObjId = UAVObjGetID(obj); connection->respInstId = instId; ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); @@ -376,8 +377,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->rxPacketLength = 1; iproc->rxCount = 0; - iproc->type = 0; - iproc->state = UAVTALK_STATE_TYPE; + iproc->type = 0; + iproc->state = UAVTALK_STATE_TYPE; break; case UAVTALK_STATE_TYPE: @@ -389,9 +390,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - iproc->type = rxbyte; + iproc->type = rxbyte; iproc->packet_size = 0; iproc->state = UAVTALK_STATE_SIZE; @@ -408,7 +409,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } iproc->packet_size += rxbyte << 8; - iproc->rxCount = 0; + iproc->rxCount = 0; if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size @@ -417,8 +418,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } - iproc->objId = 0; - iproc->state = UAVTALK_STATE_OBJID; + iproc->objId = 0; + iproc->state = UAVTALK_STATE_OBJID; break; case UAVTALK_STATE_OBJID: @@ -432,8 +433,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } iproc->rxCount = 0; - iproc->instId = 0; - iproc->state = UAVTALK_STATE_INSTID; + iproc->instId = 0; + iproc->state = UAVTALK_STATE_INSTID; break; case UAVTALK_STATE_INSTID: @@ -482,7 +483,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle if (iproc->type & UAVTALK_TIMESTAMPED) { // If there is a timestamp get it iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; + iproc->state = UAVTALK_STATE_TIMESTAMP; } else { // If there is a payload get it, otherwise receive checksum if (iproc->length > 0) { @@ -598,6 +599,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // The input packet must be completely parsed. if (inIproc->state != UAVTALK_STATE_COMPLETE) { inConnection->stats.rxErrors++; + return -1; } @@ -606,6 +608,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC if (!outConnection->outStream) { outConnection->stats.txErrors++; + return -1; } @@ -654,7 +657,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // evaluate return value before releasing the lock UAVTalkRxState rxState = 0; - if (rc != (int32_t) (headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) { + if (rc != (int32_t)(headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) { outConnection->stats.txErrors++; rxState = -1; } @@ -718,7 +721,8 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) * \return 0 Success * \return -1 Failure */ -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data) { +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data) +{ UAVObjHandle obj; int32_t ret = 0; @@ -744,8 +748,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, ui // any OBJ message can ack a pending OBJ_REQ message // even one that was not sent in response to the OBJ_REQ message updateAck(connection, type, objId, instId); - } - else { + } else { ret = -1; } } else { @@ -838,8 +841,7 @@ static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t // last instance received, complete transaction xSemaphoreGive(connection->respSema); connection->respObjId = 0; - } - else if (connection->respInstId == instId) { + } else if (connection->respInstId == instId) { xSemaphoreGive(connection->respSema); connection->respObjId = 0; } @@ -876,7 +878,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 numInst = UAVObjGetNumInstances(obj); // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) - ret = 0; + ret = 0; for (n = 0; n < numInst; ++n) { if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) { ret = -1; @@ -903,7 +905,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 * \param[in] type Transaction type * \param[in] objId The object ID * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use -() instead) + () instead) * \param[in] obj Object handle to send (null when type is NACK) * \return 0 Success * \return -1 Failure @@ -978,8 +980,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, ++connection->stats.txObjects; connection->stats.txObjectBytes += length; connection->stats.txBytes += tx_msg_len; - } - else { + } else { connection->stats.txErrors++; // TDOD rc == -1 connection not open, -2 buffer full should retry connection->stats.txBytes += (rc > 0) ? rc : 0; diff --git a/ground/openpilotgcs/src/libs/utils/crc.h b/ground/openpilotgcs/src/libs/utils/crc.h index 2d9fc15a1..92c007a59 100644 --- a/ground/openpilotgcs/src/libs/utils/crc.h +++ b/ground/openpilotgcs/src/libs/utils/crc.h @@ -32,9 +32,7 @@ #include "utils_global.h" namespace Utils { - class QTCREATOR_UTILS_EXPORT Crc { - public: /** * Update the crc value with new data. @@ -54,9 +52,7 @@ public: * \return The updated crc value. */ static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); - }; - } // namespace Utils #endif // CRC_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp index 898e22802..4eee0438e 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp @@ -128,6 +128,7 @@ void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* confi void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj) { TelemetryManager *telMngr = qobject_cast(obj); + if (telMngr) { connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); } diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index ad269d626..674f4e092 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -30,8 +30,7 @@ #include flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent) -{ -} +{} int flightDataModel::rowCount(const QModelIndex & /*parent*/) const { @@ -75,6 +74,7 @@ QVariant flightDataModel::data(const QModelIndex &index, int role) const bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value) { bool b; + switch (index) { case WPDESCRITPTION: row->wpDescritption = value.toString(); @@ -178,6 +178,7 @@ bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const { QVariant value; + switch (index) { case WPDESCRITPTION: value = row->wpDescritption; @@ -255,6 +256,7 @@ QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int in QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const { QVariant value; + if (role == Qt::DisplayRole) { if (orientation == Qt::Vertical) { value = QString::number(section + 1); @@ -605,7 +607,7 @@ void flightDataModel::readFromFile(QString fileName) while (!fieldNode.isNull()) { QDomElement field = fieldNode.toElement(); if (field.tagName() == "field") { - QString name = field.attribute("name"); + QString name = field.attribute("name"); QString value = field.attribute("value"); if (name == "altitude") { data->altitude = value.toDouble(); diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 8b31c1d65..e328bf0db 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -32,13 +32,14 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); - objMngr = pm->getObject(); + objMngr = pm->getObject(); Q_ASSERT(objMngr != NULL); completionCountdown = 0; - successCountdown = 0; + successCountdown = 0; } void ModelUavoProxy::sendPathPlan() @@ -59,7 +60,7 @@ void ModelUavoProxy::sendPathPlan() // we will start 3 update all completionCountdown = 3; - successCountdown = completionCountdown; + successCountdown = completionCountdown; pathPlan->updated(); waypoint->updatedAll(); @@ -77,8 +78,7 @@ void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success) qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0); if (successCountdown == 0) { QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful.")); - } - else { + } else { QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !")); } } @@ -87,6 +87,7 @@ void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success) void ModelUavoProxy::receivePathPlan() { PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0); + connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool))); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); @@ -97,7 +98,7 @@ void ModelUavoProxy::receivePathPlan() // we will start 3 update requests completionCountdown = 3; - successCountdown = completionCountdown; + successCountdown = completionCountdown; pathPlan->requestUpdate(); waypoint->requestUpdateAll(); @@ -117,8 +118,7 @@ void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success) if (objectsToModel()) { QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful.")); } - } - else { + } else { QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !")); } } @@ -139,12 +139,11 @@ bool ModelUavoProxy::modelToObjects() qDebug() << "ModelUAVProxy::modelToObjects"; // track number of path actions - int actionCount = 0; + int actionCount = 0; // iterate over waypoints int waypointCount = myModel->rowCount(); for (int i = 0; i < waypointCount; ++i) { - // Path Actions // create action to use as a search criteria @@ -168,8 +167,7 @@ bool ModelUavoProxy::modelToObjects() // update UAVObject action->setData(actionData); - } - else { + } else { action->deleteLater(); action = foundAction; qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID(); @@ -208,7 +206,7 @@ bool ModelUavoProxy::modelToObjects() PathPlan *pathPlan = PathPlan::GetInstance(objMngr); PathPlan::DataFields pathPlanData = pathPlan->getData(); - pathPlanData.WaypointCount = waypointCount; + pathPlanData.WaypointCount = waypointCount; pathPlanData.PathActionCount = actionCount; pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount); @@ -217,9 +215,11 @@ bool ModelUavoProxy::modelToObjects() return true; } -Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { +Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) +{ Waypoint *waypoint = NULL; int count = objMngr->getNumInstances(Waypoint::OBJID); + if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; @@ -234,17 +234,18 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { waypoint = newWaypoint ? newWaypoint : new Waypoint; waypoint->initialize(index, waypoint->getMetaObject()); objMngr->registerObject(waypoint); - } - else { + } else { // we can only create the "next" object // TODO fail in a clean way :( } return waypoint; } -PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { +PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) +{ PathAction *action = NULL; int count = objMngr->getNumInstances(PathAction::OBJID); + if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; @@ -259,17 +260,18 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { action = newAction ? newAction : new PathAction; action->initialize(index, action->getMetaObject()); objMngr->registerObject(action); - } - else { + } else { // we can only create the "next" object // TODO fail in a clean way :( } return action; } -PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { +PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) +{ int instancesCount = objMngr->getNumInstances(PathAction::OBJID); int count = actionCount <= instancesCount ? actionCount : instancesCount; + for (int i = 0; i < count; ++i) { PathAction *action = PathAction::GetInstance(objMngr, i); Q_ASSERT(action); @@ -278,15 +280,15 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD } PathAction::DataFields fields = action->getData(); if (fields.Command == actionData.Command - && fields.ConditionParameters[0] == actionData.ConditionParameters[0] - && fields.ConditionParameters[1] == actionData.ConditionParameters[1] - && fields.ConditionParameters[2] == actionData.ConditionParameters[2] - && fields.EndCondition == actionData.EndCondition - && fields.ErrorDestination == actionData.ErrorDestination - && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode - && fields.ModeParameters[0] == actionData.ModeParameters[0] - && fields.ModeParameters[1] == actionData.ModeParameters[1] - && fields.ModeParameters[2] == actionData.ModeParameters[2]) { + && fields.ConditionParameters[0] == actionData.ConditionParameters[0] + && fields.ConditionParameters[1] == actionData.ConditionParameters[1] + && fields.ConditionParameters[2] == actionData.ConditionParameters[2] + && fields.EndCondition == actionData.EndCondition + && fields.ErrorDestination == actionData.ErrorDestination + && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode + && fields.ModeParameters[0] == actionData.ModeParameters[0] + && fields.ModeParameters[1] == actionData.ModeParameters[1] + && fields.ModeParameters[2] == actionData.ModeParameters[2]) { return action; } } @@ -302,8 +304,8 @@ bool ModelUavoProxy::objectsToModel() PathPlan *pathPlan = PathPlan::GetInstance(objMngr); PathPlan::DataFields pathPlanData = pathPlan->getData(); - int waypointCount = pathPlanData.WaypointCount; - int actionCount = pathPlanData.PathActionCount; + int waypointCount = pathPlanData.WaypointCount; + int actionCount = pathPlanData.PathActionCount; // consistency check if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) { @@ -322,8 +324,7 @@ bool ModelUavoProxy::objectsToModel() int rowCount = myModel->rowCount(); if (waypointCount < rowCount) { myModel->removeRows(waypointCount, rowCount - waypointCount); - } - else if (waypointCount > rowCount) { + } else if (waypointCount > rowCount) { myModel->insertRows(rowCount, waypointCount - rowCount); } @@ -349,10 +350,12 @@ bool ModelUavoProxy::objectsToModel() return true; } -void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { +void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) +{ double distance, bearing, altitude, velocity; QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE); + distance = myModel->data(index).toDouble(); index = myModel->index(i, flightDataModel::BEARELATIVE); bearing = myModel->data(index).toDouble(); @@ -367,59 +370,64 @@ void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { data.Velocity = velocity; } -void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) { - +void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) +{ double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] + - data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]); + data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]); double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; + if (bearing != bearing) { bearing = 0; } - double altitude = -data.Position[Waypoint::POSITION_DOWN]; + double altitude = -data.Position[Waypoint::POSITION_DOWN]; QModelIndex index = myModel->index(i, flightDataModel::VELOCITY); myModel->setData(index, data.Velocity); - index = myModel->index(i, flightDataModel::DISRELATIVE); + index = myModel->index(i, flightDataModel::DISRELATIVE); myModel->setData(index, distance); - index = myModel->index(i, flightDataModel::BEARELATIVE); + index = myModel->index(i, flightDataModel::BEARELATIVE); myModel->setData(index, bearing); - index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); + index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); myModel->setData(index, altitude); } -void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data) { +void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data) +{ QModelIndex index = myModel->index(i, flightDataModel::MODE); - data.Mode = myModel->data(index).toInt(); - index = myModel->index(i, flightDataModel::MODE_PARAMS0); + + data.Mode = myModel->data(index).toInt(); + index = myModel->index(i, flightDataModel::MODE_PARAMS0); data.ModeParameters[0] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::MODE_PARAMS1); + index = myModel->index(i, flightDataModel::MODE_PARAMS1); data.ModeParameters[1] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::MODE_PARAMS2); + index = myModel->index(i, flightDataModel::MODE_PARAMS2); data.ModeParameters[2] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::MODE_PARAMS3); + index = myModel->index(i, flightDataModel::MODE_PARAMS3); data.ModeParameters[3] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::CONDITION); + index = myModel->index(i, flightDataModel::CONDITION); data.EndCondition = myModel->data(index).toInt(); - index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); data.ConditionParameters[0] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::CONDITION_PARAMS1); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS1); data.ConditionParameters[1] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); data.ConditionParameters[2] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::CONDITION_PARAMS3); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS3); data.ConditionParameters[3] = myModel->data(index).toFloat(); - index = myModel->index(i, flightDataModel::COMMAND); + index = myModel->index(i, flightDataModel::COMMAND); data.Command = myModel->data(index).toInt(); - index = myModel->index(i, flightDataModel::JUMPDESTINATION); + index = myModel->index(i, flightDataModel::JUMPDESTINATION); data.JumpDestination = myModel->data(index).toInt() - 1; - index = myModel->index(i, flightDataModel::ERRORDESTINATION); + index = myModel->index(i, flightDataModel::ERRORDESTINATION); data.ErrorDestination = myModel->data(index).toInt() - 1; } -void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { +void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) +{ QModelIndex index = myModel->index(i, flightDataModel::ISRELATIVE); + myModel->setData(index, true); index = myModel->index(i, flightDataModel::COMMAND); @@ -456,16 +464,17 @@ void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { myModel->setData(index, data.ModeParameters[3]); } -quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount) { +quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount) +{ quint8 crc = 0; + for (int i = 0; i < waypointCount; ++i) { - Waypoint* waypoint = Waypoint::GetInstance(objMngr, i); + Waypoint *waypoint = Waypoint::GetInstance(objMngr, i); crc = waypoint->updateCRC(crc); } for (int i = 0; i < actionCount; ++i) { - PathAction* action = PathAction::GetInstance(objMngr, i); + PathAction *action = PathAction::GetInstance(objMngr, i); crc = action->updateCRC(crc); } return crc; } - diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index d62d67746..e1f26262f 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -58,7 +58,7 @@ private: Waypoint *createWaypoint(int index, Waypoint *newWaypoint); PathAction *createPathAction(int index, PathAction *newAction); - PathAction *findPathAction(const PathAction::DataFields& actionFields, int actionCount); + PathAction *findPathAction(const PathAction::DataFields & actionFields, int actionCount); void modelToWaypoint(int i, Waypoint::DataFields &data); void modelToPathAction(int i, PathAction::DataFields &data); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 275a9708b..90a42be9e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -55,13 +55,13 @@ using namespace Utils; */ UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name) { - this->objID = objID; - this->instID = 0; + this->objID = objID; + this->instID = 0; this->isSingleInst = isSingleInst; - this->name = name; - this->data = 0; - this->numBytes = 0; - this->mutex = new QMutex(QMutex::Recursive); + this->name = name; + this->data = 0; + this->numBytes = 0; + this->mutex = new QMutex(QMutex::Recursive); } /** @@ -215,7 +215,7 @@ void UAVObject::updatedAll() if (instID == 0) { emit objectUpdatedManual(this, true); // TODO call objectUpdated() for all instances? - //emit objectUpdated(this); + // emit objectUpdated(this); } } @@ -287,7 +287,7 @@ UAVObjectField *UAVObject::getField(const QString & name) } // If this point is reached then the field was not found qWarning() << "UAVObject::getField Non existant field" << name << "requested." - << "This indicates a bug. Make sure you also have null checking for non-debug code."; + << "This indicates a bug. Make sure you also have null checking for non-debug code."; return NULL; } @@ -334,8 +334,8 @@ quint8 UAVObject::updateCRC(quint8 crc) { QMutexLocker locker(mutex); - //crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID)); - //crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID)); + // crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID)); + // crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID)); crc = Crc::updateCRC(crc, data, numBytes); return crc; diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 31435fca6..5b3b402fe 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -257,6 +257,7 @@ int UAVObjectUtilManager::getBoardModel() FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); int ret = firmwareIapData.BoardType << 8; + ret = ret + firmwareIapData.BoardRevision; return ret; diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index b9317114c..70aac4163 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -1230,6 +1230,7 @@ void ConfigTaskWidget::loadWidgetLimits(QWidget *widget, UAVObjectField *field, void ConfigTaskWidget::updateEnableControls() { TelemetryManager *telMngr = pm->getObject(); + Q_ASSERT(telMngr); enableControls(telMngr->isConnected()); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 6ce88acbe..059d988b1 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -230,6 +230,7 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success) { // Lookup the transaction in the transaction map. ObjectTransactionInfo *transInfo = findTransaction(obj); + if (transInfo) { if (success) { #ifdef VERBOSE_TELEMETRY @@ -264,17 +265,17 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) #endif ++txRetries; --transInfo->retriesRemaining; - + // Retry the transaction processObjectTransaction(transInfo); } else { qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief(); ++txErrors; - + // Terminate transaction utalk->cancelTransaction(transInfo->obj); - + // Remove this transaction as it's complete. UAVObject *obj = transInfo->obj; closeTransaction(transInfo); @@ -294,6 +295,7 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) { // Initiate transaction bool sent = false; + if (transInfo->objRequest) { #ifdef VERBOSE_TELEMETRY qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); @@ -310,14 +312,12 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) if (sent) { // Start timer if a response is expected transInfo->timer->start(REQ_TIMEOUT_MS); - } - else { + } else { // message was not sent, the transaction will not complete and will timeout // there is no need to wait to close the transaction and notify of completion failure - //transactionCompleted(transInfo->obj, false); + // transactionCompleted(transInfo->obj, false); } - } - else { + } else { // not transacted, so just close the transaction with no notification of completion closeTransaction(transInfo); } @@ -378,8 +378,8 @@ void Telemetry::processObjectQueue() if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) { objQueue.clear(); if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) && - (objInfo.obj->getObjID() != OPLinkSettings::OBJID) && - (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) { + (objInfo.obj->getObjID() != OPLinkSettings::OBJID) && + (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) { objInfo.obj->emitTransactionCompleted(false); return; } @@ -396,7 +396,7 @@ void Telemetry::processObjectQueue() // TODO make the above logic a reality... if (findTransaction(objInfo.obj)) { qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress"; - //objInfo.obj->emitTransactionCompleted(false); + // objInfo.obj->emitTransactionCompleted(false); return; } UAVObject::Metadata metadata = objInfo.obj->getMetadata(); @@ -466,7 +466,7 @@ void Telemetry::processPeriodicUpdates() time.start(); allInstances = !objinfo->obj->isSingleInstance(); processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false); - elapsedMs = time.elapsed(); + elapsedMs = time.elapsed(); // Update timeToNextUpdateMs with the elapsed delay of sending the object; timeToNextUpdateMs += elapsedMs; } @@ -537,6 +537,7 @@ void Telemetry::objectUpdatedManual(UAVObject *obj, bool all) QMutexLocker locker(mutex); bool allInstances = obj->isSingleInstance() ? false : all; + processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true); } @@ -559,6 +560,7 @@ void Telemetry::updateRequested(UAVObject *obj, bool all) QMutexLocker locker(mutex); bool allInstances = obj->isSingleInstance() ? false : all; + processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true); } @@ -578,7 +580,7 @@ void Telemetry::newInstance(UAVObject *obj) ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) { - quint32 objId = obj->getObjID(); + quint32 objId = obj->getObjID(); quint16 instId = obj->getInstID(); // Lookup the transaction in the transaction map @@ -596,7 +598,7 @@ ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) void Telemetry::openTransaction(ObjectTransactionInfo *trans) { - quint32 objId = trans->obj->getObjID(); + quint32 objId = trans->obj->getObjID(); quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID(); QMap *objTransactions = transMap.value(objId); @@ -609,7 +611,7 @@ void Telemetry::openTransaction(ObjectTransactionInfo *trans) void Telemetry::closeTransaction(ObjectTransactionInfo *trans) { - quint32 objId = trans->obj->getObjID(); + quint32 objId = trans->obj->getObjID(); quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID(); QMap *objTransactions = transMap.value(objId); @@ -627,6 +629,7 @@ void Telemetry::closeAllTransactions() QMap *objTransactions = transMap.value(objId); foreach(quint32 instId, objTransactions->keys()) { ObjectTransactionInfo *trans = objTransactions->value(instId); + qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief(); objTransactions->remove(instId); delete trans; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp index ad3b3db16..d0865337c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp @@ -43,8 +43,7 @@ TelemetryManager::TelemetryManager() : autopilotConnected(false) } TelemetryManager::~TelemetryManager() -{ -} +{} bool TelemetryManager::isConnected() { @@ -76,8 +75,7 @@ void TelemetryManager::onStart() connect(device, SIGNAL(readyRead()), reader, SLOT(read())); // start the reader thread readerThread.start(); - } - else { + } else { // Connect IO device to reader connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream())); } @@ -127,8 +125,7 @@ void TelemetryManager::onTelemetryUpdate(double txRate, double rxRate) } IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk) -{ -} +{} void IODeviceReader::read() { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h index fd7745c68..0e32fcf55 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h @@ -72,8 +72,7 @@ private: }; -class IODeviceReader : public QObject -{ +class IODeviceReader : public QObject { Q_OBJECT public: IODeviceReader(UAVTalk *uavTalk); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp index f0068c6cf..ad1148654 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp @@ -199,16 +199,16 @@ void TelemetryMonitor::processStatsUpdates() tel->resetStats(); // Update stats object - gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); - gcsStats.TxBytes += telStats.txBytes; - gcsStats.TxFailures += telStats.txErrors; - gcsStats.TxRetries += telStats.txRetries; + gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.TxBytes += telStats.txBytes; + gcsStats.TxFailures += telStats.txErrors; + gcsStats.TxRetries += telStats.txRetries; - gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); - gcsStats.RxBytes += telStats.rxBytes; - gcsStats.RxFailures += telStats.rxErrors; + gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.RxBytes += telStats.rxBytes; + gcsStats.RxFailures += telStats.rxErrors; gcsStats.RxSyncErrors += telStats.rxSyncErrors; - gcsStats.RxCrcErrors += telStats.rxCrcErrors; + gcsStats.RxCrcErrors += telStats.rxCrcErrors; // Check for a connection timeout bool connectionTimeout; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 1c7a33322..08ac75935 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -35,8 +35,8 @@ #ifdef VERBOSE_UAVTALK // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only -//#include "flighttelemetrystats.h" -//#define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID) +// #include "flighttelemetrystats.h" +// #define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID) #endif #ifndef VERBOSE_FILTER #define VERBOSE_FILTER(objId) @@ -124,9 +124,8 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) if (allInstances) { instId = ALL_INSTANCES; - } - else if (obj) { - instId = obj->getInstID(); + } else if (obj) { + instId = obj->getInstID(); } bool success = false; if (acked) { @@ -153,9 +152,8 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) if (allInstances) { instId = ALL_INSTANCES; - } - else if (obj) { - instId = obj->getInstID(); + } else if (obj) { + instId = obj->getInstID(); } return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj); } @@ -188,7 +186,7 @@ void UAVTalk::cancelTransaction(UAVObject *obj) */ bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { - Q_ASSERT(obj); + Q_ASSERT(obj); // Send object depending on if a response is needed // transactions of TYPE_OBJ_REQ are acked by the response if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { @@ -217,8 +215,7 @@ void UAVTalk::processInputStream() int ret = io->read((char *)&tmp, 1); if (ret != -1) { processInputByte(tmp); - } - else { + } else { // TODOD } if (rxState == STATE_COMPLETE) { @@ -226,8 +223,7 @@ void UAVTalk::processInputStream() if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) { stats.rxObjectBytes += rxLength; stats.rxObjects++; - } - else { + } else { // TODO... } mutex.unlock(); @@ -237,7 +233,6 @@ void UAVTalk::processInputStream() // accessed from this thread only udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); } - } } } @@ -319,7 +314,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) break; } packetSize += (quint32)rxbyte << 8; - rxCount = 0; + rxCount = 0; if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { @@ -342,13 +337,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCount < 4) { break; } - rxCount = 0; + rxCount = 0; - rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); + rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); // Message always contain an instance ID rxInstId = 0; - rxState = STATE_INSTID; + rxState = STATE_INSTID; break; case STATE_INSTID: @@ -402,7 +397,6 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxState = STATE_ERROR; break; } - } // If there is a payload get it, otherwise receive checksum @@ -651,15 +645,13 @@ void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *o if (trans && trans->respType == type) { if (trans->respInstId == ALL_INSTANCES) { if (instId == 0) { - // last instance received, complete transaction + // last instance received, complete transaction closeTransaction(trans); emit transactionCompleted(obj, true); + } else { + // TODO extend timeout? } - else { - // TODO extend timeout? - } - } - else { + } else { closeTransaction(trans); emit transactionCompleted(obj, true); } @@ -713,7 +705,7 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje ret = true; quint32 numInst = objMngr->getNumInstances(objId); for (quint32 n = 0; n < numInst; ++n) { - quint32 i = numInst - n - 1; + quint32 i = numInst - n - 1; UAVObject *o = objMngr->getObject(objId, i); if (!transmitSingleObject(type, objId, i, o)) { ret = false; @@ -800,13 +792,11 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U if (useUDPMirror) { udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); } - } - else { + } else { qWarning() << "UAVTalk - error transmitting : io device full"; ++stats.txErrors; return false; } - } else { qWarning() << "UAVTalk - error transmitting : io device not writable"; ++stats.txErrors; @@ -840,8 +830,9 @@ UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId) void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId) { Transaction *trans = new Transaction(); - trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK; - trans->respObjId = objId; + + trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK; + trans->respObjId = objId; trans->respInstId = instId; QMap *objTransactions = transMap.value(trans->respObjId); @@ -869,6 +860,7 @@ void UAVTalk::closeAllTransactions() QMap *objTransactions = transMap.value(objId); foreach(quint32 instId, objTransactions->keys()) { Transaction *trans = objTransactions->value(instId); + qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId; objTransactions->remove(instId); delete trans; @@ -878,29 +870,33 @@ void UAVTalk::closeAllTransactions() } } -const char* UAVTalk::typeToString(quint8 type) +const char *UAVTalk::typeToString(quint8 type) { switch (type) { case TYPE_OBJ: return "object"; + break; case TYPE_OBJ_ACK: return "object (acked)"; + break; case TYPE_OBJ_REQ: return "object request"; + break; case TYPE_ACK: return "ack"; + break; case TYPE_NACK: return "nack"; - break; + break; } return ""; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index fe5922096..4d27399f9 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -44,7 +44,7 @@ class UAVTALK_EXPORT UAVTalk : public QObject { friend class IODeviceReader; public: - static const quint16 ALL_INSTANCES = 0xFFFF; + static const quint16 ALL_INSTANCES = 0xFFFF; typedef struct { quint32 txBytes; @@ -80,22 +80,22 @@ private slots: private: typedef struct { - quint8 respType; + quint8 respType; quint32 respObjId; quint16 respInstId; } Transaction; // Constants - static const int TYPE_MASK = 0xF8; - static const int TYPE_VER = 0x20; - static const int TYPE_OBJ = (TYPE_VER | 0x00); - static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); - static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); - static const int TYPE_ACK = (TYPE_VER | 0x03); - static const int TYPE_NACK = (TYPE_VER | 0x04); + static const int TYPE_MASK = 0xF8; + static const int TYPE_VER = 0x20; + static const int TYPE_OBJ = (TYPE_VER | 0x00); + static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); + static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); + static const int TYPE_ACK = (TYPE_VER | 0x03); + static const int TYPE_NACK = (TYPE_VER | 0x04); // header : sync(1), type (1), size(2), object ID(4), instance ID(2) - static const int HEADER_LENGTH = 10; + static const int HEADER_LENGTH = 10; static const int MAX_PAYLOAD_LENGTH = 256; From 92beb54e2dff6c922eb68246cb68d333b8b4fd05 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 15 Jan 2014 18:43:47 +0100 Subject: [PATCH 110/113] OP-1022 some small cosmetic fixes for AltitudeHold as suggested by Alessio --- flight/libraries/sanitycheck.c | 2 ++ flight/modules/AltitudeHold/altitudehold.c | 16 ++++++++-------- flight/modules/ManualControl/manualcontrol.c | 16 ++++++++-------- .../boards/revoproto/firmware/pios_board.c | 2 +- .../uavobjectdefinition/altitudeholddesired.xml | 4 ++-- 5 files changed, 21 insertions(+), 19 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index ab53e2302..8d876a258 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -114,6 +114,8 @@ int32_t configuration_check() if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } + // TODO: put check equivalent to TASK_MONITOR_IsRunning + // here as soon as available for delayed callbacks break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: if (coptercontrol) { diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 6a81518d6..3d71ee2b2 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -146,13 +146,13 @@ static void altitudeHoldTask(void) float velocityStateDown; VelocityStateDownGet(&velocityStateDown); - switch (altitudeHoldDesired.ThrottleMode) { - case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE: + switch (altitudeHoldDesired.ControlMode) { + case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE: // altitude control loop - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.ThrottleCommand, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); break; - case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY: - altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand; + case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY: + altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint; break; default: altitudeHoldStatus.VelocityDesired = 0; @@ -162,9 +162,9 @@ static void altitudeHoldTask(void) AltitudeHoldStatusSet(&altitudeHoldStatus); float throttle; - switch (altitudeHoldDesired.ThrottleMode) { - case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE: - throttle = altitudeHoldDesired.ThrottleCommand; + switch (altitudeHoldDesired.ControlMode) { + case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE: + throttle = altitudeHoldDesired.SetPoint; break; default: // velocity control loop diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 7dcfb32ea..35cfc525c 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -882,22 +882,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); if (cutOff && cmd->Throttle < 0) { // Cut throttle if desired - altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle; - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE; + altitudeHoldDesiredData.SetPoint = cmd->Throttle; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE; newaltitude = true; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.ThrottleCommand = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; newaltitude = true; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { - altitudeHoldDesiredData.ThrottleCommand = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; newaltitude = true; } else if (newaltitude == true) { - altitudeHoldDesiredData.ThrottleCommand = posState.Down; - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE; + altitudeHoldDesiredData.SetPoint = posState.Down; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; newaltitude = false; } diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b3a76c386..ed142f951 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -129,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_4096, }; #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 0dfe36ceb..1b349766f 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,8 +1,8 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through - - + + From eb8d57cf3159b16311145430453cf8d0e5d7b627 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 15 Jan 2014 18:47:47 +0100 Subject: [PATCH 111/113] uncrustification --- .../src/libs/utils/mytabbedstackwidget.cpp | 1 + .../src/libs/utils/mytabbedstackwidget.h | 2 +- .../uavobjectwidgetutils/configtaskwidget.cpp | 12 ++++++------ 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index e465b50f9..2626ca47b 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -137,6 +137,7 @@ void MyTabbedStackWidget::showWidget(int index) void MyTabbedStackWidget::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); + m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH); } diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index 4b0777e4e..661200383 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -79,7 +79,7 @@ private: static const int LIST_VIEW_WIDTH = 80; protected: - void resizeEvent(QResizeEvent * event); + void resizeEvent(QResizeEvent *event); }; #endif // MYTABBEDSTACKWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index 12f58a50e..5c2817dcc 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -667,7 +667,7 @@ void ConfigTaskWidget::autoLoadWidgets() forceShadowUpdates(); /* - foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { + foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { if (binding->widget()) { qDebug() << "Binding :" << binding->widget()->objectName(); qDebug() << " Object :" << binding->object()->getName(); @@ -681,8 +681,8 @@ void ConfigTaskWidget::autoLoadWidgets() qDebug() << " Scale :" << shadow->scale(); } } - } - */ + } + */ } void ConfigTaskWidget::addWidgetToReloadGroups(QWidget *widget, QList *reloadGroupIDs) @@ -1157,10 +1157,10 @@ void WidgetBinding::setValue(const QVariant &value) { m_value = value; /* - if (m_object && m_field) { + if (m_object && m_field) { qDebug() << "WidgetBinding" << m_object->getName() << ":" << m_field->getName() << "value =" << value.toString(); - } - */ + } + */ } void WidgetBinding::updateObjectFieldFromValue() From 3f66d656cac935781b216d5429dd05d68f544d73 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Wed, 15 Jan 2014 20:04:54 +0100 Subject: [PATCH 112/113] OP-1122 OP-1158 - renamed uavobject FlightPlan to PathPlan --- flight/targets/boards/simposix/firmware/UAVObjects.inc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 59feac088..b79a09448 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -49,7 +49,6 @@ UAVOBJSRCFILENAMES += debuglogentry UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate -UAVOBJSRCFILENAMES += flightplan UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus @@ -74,6 +73,7 @@ UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired From b69621ba1110fa92ecb700e464acd5fc29586d0b Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 16 Jan 2014 19:01:44 +0100 Subject: [PATCH 113/113] OP-984 fixed jumping PID Tab in stabilization config panel --- .../src/plugins/config/stabilization.ui | 121 +++--------------- 1 file changed, 18 insertions(+), 103 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 6a4fa8186..1acff9244 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -559,9 +559,6 @@ 0 - - 9 - @@ -573,7 +570,7 @@ #basicPidBankFrame{ color: rgb(180, 180, 180); -margin-top: -1px +margin-top: -1px; } @@ -8617,16 +8614,16 @@ border-radius: 5; - 9 + 0 - 9 + 0 - 9 + 0 - 9 + 0 @@ -8718,7 +8715,7 @@ border-radius: 5; 20 - 40 + 20 @@ -8766,9 +8763,6 @@ border-radius: 5; 708 - - true - 0 @@ -8791,9 +8785,6 @@ border-radius: 5; QFrame::Plain - - 9 - @@ -18664,7 +18655,7 @@ border-radius: 5; - + 0 0 @@ -18672,16 +18663,13 @@ border-radius: 5; 0 - 60 + 0 Instant Update - - 9 - @@ -18710,19 +18698,6 @@ border-radius: 5; - - - - Qt::Vertical - - - - 20 - 40 - - - - @@ -18731,7 +18706,7 @@ border-radius: 5; 20 - 200 + 20 @@ -18775,26 +18750,14 @@ border-radius: 5; 0 0 - 796 - 708 + 752 + 526 0 - - 9 - - - 9 - - - 9 - - - 9 - @@ -21272,18 +21235,6 @@ border-radius: 5; - - - 0 - 0 - - - - - 0 - 0 - - false @@ -21355,7 +21306,7 @@ border-radius: 5; - + QGroupBox{border: 0px;} @@ -23914,24 +23865,6 @@ border-radius: 5; - - - 0 - 0 - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - @@ -24452,9 +24385,6 @@ border-radius: 5; Sensor Tuning - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - 9 @@ -26859,7 +26789,7 @@ border-radius: 5; - 40 + 20 9 @@ -26867,18 +26797,6 @@ border-radius: 5; - - - 0 - 0 - - - - - 0 - 60 - - Instant Update @@ -26919,7 +26837,7 @@ border-radius: 5; 20 - 149 + 20 @@ -26963,8 +26881,8 @@ border-radius: 5; 0 0 - 796 - 708 + 284 + 133 @@ -27215,14 +27133,11 @@ border-radius: 5; 0 0 - 796 - 708 + 347 + 500 - - 9 -