diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 8996433a3..1cda21991 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -206,6 +206,9 @@ int main() gps_data.quality = -1; uint32_t last_gps_time = 0; uint32_t last_indoor_time = 0; + uint32_t up_time_real = 0; + uint32_t up_time = 0; + uint32_t last_up_time = 0; ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE; @@ -311,9 +314,16 @@ for all data to be up to date before doing anything*/ AhrsStatusGet(&status); status.CPULoad = ((float)running_counts / (float)(idle_counts + running_counts)) * 100; - status.IdleTimePerCyle = idle_counts / (TIMER_RATE / 10000); - status.RunningTimePerCyle = running_counts / (TIMER_RATE / 10000); + status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000); + status.RunningTimePerCyle = running_counts / (timer_rate() / 10000); status.DroppedUpdates = ekf_too_slow; + up_time = timer_count(); + if(up_time >= last_up_time) // normal condition + up_time_real += ((up_time - last_up_time) * 1000) / timer_rate(); + else + up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate(); + last_up_time = up_time; + status.RunningTime = up_time_real; AhrsStatusSet(&status); // Alive signal diff --git a/flight/OpenPilot/UAVObjects/inc/ahrsstatus.h b/flight/OpenPilot/UAVObjects/inc/ahrsstatus.h index 4c2e53ad3..43b59c623 100644 --- a/flight/OpenPilot/UAVObjects/inc/ahrsstatus.h +++ b/flight/OpenPilot/UAVObjects/inc/ahrsstatus.h @@ -41,7 +41,7 @@ #define AHRSSTATUS_H // Object constants -#define AHRSSTATUS_OBJID 3773703778U +#define AHRSSTATUS_OBJID 933623714U #define AHRSSTATUS_NAME "AhrsStatus" #define AHRSSTATUS_METANAME "AhrsStatusMeta" #define AHRSSTATUS_ISSINGLEINST 1 @@ -73,6 +73,7 @@ typedef struct { uint8_t SerialNumber[8]; uint8_t CPULoad; + uint32_t RunningTime; uint8_t IdleTimePerCyle; uint8_t RunningTimePerCyle; uint8_t DroppedUpdates; @@ -92,6 +93,7 @@ typedef struct { /* Number of elements for field SerialNumber */ #define AHRSSTATUS_SERIALNUMBER_NUMELEM 8 // Field CPULoad information +// Field RunningTime information // Field IdleTimePerCyle information // Field RunningTimePerCyle information // Field DroppedUpdates information diff --git a/ground/src/plugins/uavobjects/ahrsstatus.cpp b/ground/src/plugins/uavobjects/ahrsstatus.cpp index cfa4fcabc..febc2b8b5 100644 --- a/ground/src/plugins/uavobjects/ahrsstatus.cpp +++ b/ground/src/plugins/uavobjects/ahrsstatus.cpp @@ -55,6 +55,9 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME) QStringList CPULoadElemNames; CPULoadElemNames.append("0"); fields.append( new UAVObjectField(QString("CPULoad"), QString("count"), UAVObjectField::UINT8, CPULoadElemNames, QStringList()) ); + QStringList RunningTimeElemNames; + RunningTimeElemNames.append("0"); + fields.append( new UAVObjectField(QString("RunningTime"), QString("ms"), UAVObjectField::UINT32, RunningTimeElemNames, QStringList()) ); QStringList IdleTimePerCyleElemNames; IdleTimePerCyleElemNames.append("0"); fields.append( new UAVObjectField(QString("IdleTimePerCyle"), QString("10x ms"), UAVObjectField::UINT8, IdleTimePerCyleElemNames, QStringList()) ); diff --git a/ground/src/plugins/uavobjects/ahrsstatus.h b/ground/src/plugins/uavobjects/ahrsstatus.h index 1feb35794..c90d115e7 100644 --- a/ground/src/plugins/uavobjects/ahrsstatus.h +++ b/ground/src/plugins/uavobjects/ahrsstatus.h @@ -45,6 +45,7 @@ public: typedef struct { quint8 SerialNumber[8]; quint8 CPULoad; + quint32 RunningTime; quint8 IdleTimePerCyle; quint8 RunningTimePerCyle; quint8 DroppedUpdates; @@ -64,6 +65,7 @@ public: /* Number of elements for field SerialNumber */ static const quint32 SERIALNUMBER_NUMELEM = 8; // Field CPULoad information + // Field RunningTime information // Field IdleTimePerCyle information // Field RunningTimePerCyle information // Field DroppedUpdates information @@ -80,7 +82,7 @@ public: // Constants - static const quint32 OBJID = 3773703778U; + static const quint32 OBJID = 933623714U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 0; diff --git a/ground/src/plugins/uavobjects/ahrsstatus.py b/ground/src/plugins/uavobjects/ahrsstatus.py index 997e7f5c8..dbfc8be91 100644 --- a/ground/src/plugins/uavobjects/ahrsstatus.py +++ b/ground/src/plugins/uavobjects/ahrsstatus.py @@ -64,6 +64,16 @@ _fields = [ \ { } ), + uavobject.UAVObjectField( + 'RunningTime', + 'I', + 1, + [ + '0', + ], + { + } + ), uavobject.UAVObjectField( 'IdleTimePerCyle', 'B', @@ -181,7 +191,7 @@ _fields = [ \ class AhrsStatus(uavobject.UAVObject): ## Object constants - OBJID = 3773703778 + OBJID = 933623714 NAME = "AhrsStatus" METANAME = "AhrsStatusMeta" ISSINGLEINST = 1 diff --git a/ground/src/shared/uavobjectdefinition/ahrsstatus.xml b/ground/src/shared/uavobjectdefinition/ahrsstatus.xml index 145ddf5b0..12658c944 100644 --- a/ground/src/shared/uavobjectdefinition/ahrsstatus.xml +++ b/ground/src/shared/uavobjectdefinition/ahrsstatus.xml @@ -3,6 +3,7 @@ Status for the @ref AHRSCommsModule, including communication errors +