diff --git a/HISTORY.txt b/HISTORY.txt index b2cfe1b92..4d22602df 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -1,5 +1,12 @@ Short summary of changes. For a complete list see the git log. +2012-06-04 +AeroSimRC support merged into next + +2012-05-26 +VirtualFlybar which allows a more aggressive flight mode than rate mode +support. Also PiroCompensation added. + 2012-05-26 Revert some UI changes that didn't work consistently between OSX and Windows. diff --git a/Makefile b/Makefile index 1e7df4211..67571d3f8 100644 --- a/Makefile +++ b/Makefile @@ -630,7 +630,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS))) all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS))) endef -ALL_BOARDS := coptercontrol pipxtreme revolution simposix +ALL_BOARDS := coptercontrol pipxtreme # Friendly names of each board (used to find source tree) coptercontrol_friendly := CopterControl diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 2db478a07..42d5a4bfe 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -122,6 +122,7 @@ static const struct flashfs_cfg flashfs_w25x_cfg = { .obj_table_start = 0x00000010, .obj_table_end = 0x00001000, .sector_size = 0x00001000, + .chip_size = 0x00080000, }; static const struct pios_flash_jedec_cfg flash_w25x_cfg = { @@ -135,6 +136,7 @@ static const struct flashfs_cfg flashfs_m25p_cfg = { .obj_table_start = 0x00000010, .obj_table_end = 0x00010000, .sector_size = 0x00010000, + .chip_size = 0x00200000, }; static const struct pios_flash_jedec_cfg flash_m25p_cfg = { diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 47604be34..02ba45744 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -234,17 +234,21 @@ static void AttitudeTask(void *parameters) AccelsData accels; GyrosData gyros; - int32_t retval; - if(cc3d) + int32_t retval = 0; + + if (cc3d) retval = updateSensorsCC3D(&accels, &gyros); else retval = updateSensors(&accels, &gyros); - if(retval != 0) + // Only update attitude when sensor data is good + if (retval != 0) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); else { - // Only update attitude when sensor data is good - updateAttitude(&accels, &gyros); + // Do not update attitude data in simulation mode + if (!AttitudeActualReadOnly()) + updateAttitude(&accels, &gyros); + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } } @@ -267,7 +271,11 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); return -1; } - + + // Do not read raw sensor data in simulation mode + if (GyrosReadOnly() || AccelsReadOnly()) + return 0; + // No accel data available if(PIOS_ADXL345_FifoElements() == 0) return -1; diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 8ffe1a77f..cc188e602 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -586,6 +586,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : @@ -593,6 +594,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : @@ -600,6 +602,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 571e6ac7a..b08a90076 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -55,7 +55,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define FAILSAFE_TIMEOUT_MS 30 -enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX}; +enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX}; enum {ROLL,PITCH,YAW,MAX_AXES}; @@ -74,21 +74,24 @@ typedef struct { static xTaskHandle taskHandle; static StabilizationSettingsData settings; static xQueueHandle queue; -float dT = 1; float gyro_alpha = 0; float gyro_filtered[3] = {0,0,0}; float axis_lock_accum[3] = {0,0,0}; +float vbar_sensitivity[3] = {1, 1, 1}; uint8_t max_axis_lock = 0; uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; - +float vbar_integral[3] = {0, 0, 0}; +float vbar_decay = 0.991f; pid_type pids[PID_MAX]; +int8_t vbar_gyros_suppress; +bool vbar_piro_comp = false; // Private functions static void stabilizationTask(void* parameters); -static float ApplyPid(pid_type * pid, const float err); +static float ApplyPid(pid_type * pid, const float err, float dT); static float bound(float val); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent * ev); @@ -154,6 +157,8 @@ static void stabilizationTask(void* parameters) // Main task loop ZeroPids(); while(1) { + float dT; + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe @@ -227,6 +232,7 @@ static void stabilizationTask(void* parameters) switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: rateDesiredAxis[i] = attitudeDesiredAxis[i]; // Zero attitude and axis lock accumulators @@ -251,7 +257,7 @@ static void stabilizationTask(void* parameters) break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]); + rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); if(rateDesiredAxis[i] > settings.MaximumRate[i]) rateDesiredAxis[i] = settings.MaximumRate[i]; @@ -275,7 +281,7 @@ static void stabilizationTask(void* parameters) else if(axis_lock_accum[i] < -max_axis_lock) axis_lock_accum[i] = -max_axis_lock; - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]); + rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } if(rateDesiredAxis[i] > settings.MaximumRate[i]) @@ -287,42 +293,78 @@ static void stabilizationTask(void* parameters) } } + // Piro compensation rotates the virtual flybar by yaw step to keep it + // rotated to external world + if (vbar_piro_comp) { + const float F_PI = 3.14159f; + float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT); + float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT); + + float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; + float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; + + vbar_integral[1] = vbar_pitch; + vbar_integral[0] = vbar_roll; + } + uint8_t shouldUpdate = 1; #if defined(DIAGNOSTICS) RateDesiredSet(&rateDesired); #endif ActuatorDesiredGet(&actuatorDesired); //Calculate desired command - for(int8_t ct=0; ct< MAX_AXES; ct++) + for(uint32_t i = 0; i < MAX_AXES; i++) { - switch(stabDesired.StabilizationMode[ct]) + switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { - float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]); - actuatorDesiredAxis[ct] = bound(command); + float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(command); + break; + } + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: + { + // Track the angle of the virtual flybar which includes a slow decay + vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT; + if (vbar_integral[i] > settings.VbarMaxAngle) + vbar_integral[i] = settings.VbarMaxAngle; + if (-vbar_integral[i] < -settings.VbarMaxAngle) + vbar_integral[i] = -settings.VbarMaxAngle; + + // Command signal is composed of stick input added to the gyro and virtual flybar + float gyro_gain = 1.0f; + if (vbar_gyros_suppress > 0) { + gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f); + gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + } + float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * ( + vbar_integral[i] * pids[PID_VBAR_ROLL + i].i + + gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p); + + actuatorDesiredAxis[i] = bound(command); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - switch (ct) + switch (i) { case ROLL: - actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); shouldUpdate = 1; pids[PID_RATE_ROLL].iAccumulator = 0; pids[PID_ROLL].iAccumulator = 0; break; case PITCH: - actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); shouldUpdate = 1; pids[PID_RATE_PITCH].iAccumulator = 0; pids[PID_PITCH].iAccumulator = 0; break; case YAW: - actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); shouldUpdate = 1; pids[PID_RATE_YAW].iAccumulator = 0; pids[PID_YAW].iAccumulator = 0; @@ -360,7 +402,7 @@ static void stabilizationTask(void* parameters) } } -float ApplyPid(pid_type * pid, const float err) +float ApplyPid(pid_type * pid, const float err, float dT) { float diff = (err - pid->lastErr); pid->lastErr = err; @@ -400,44 +442,60 @@ static float bound(float val) return val; } - static void SettingsUpdatedCb(UAVObjEvent * ev) { memset(pids,0,sizeof (pid_type) * PID_MAX); StabilizationSettingsGet(&settings); // Set the roll rate PID constants - pids[0].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP]; - pids[0].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI]; - pids[0].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD]; - pids[0].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]; + pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP]; + pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI]; + pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD]; + pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]; // Set the pitch rate PID constants - pids[1].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP]; - pids[1].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI]; - pids[1].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD]; - pids[1].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]; + pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP]; + pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI]; + pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD]; + pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]; // Set the yaw rate PID constants - pids[2].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP]; - pids[2].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI]; - pids[2].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD]; - pids[2].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]; + pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP]; + pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI]; + pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD]; + pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]; // Set the roll attitude PI constants - pids[3].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP]; - pids[3].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI]; - pids[3].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]; + pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP]; + pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI]; + pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]; // Set the pitch attitude PI constants - pids[4].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP]; - pids[4].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI]; - pids[4].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]; + pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP]; + pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI]; + pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]; // Set the yaw attitude PI constants - pids[5].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP]; - pids[5].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; - pids[5].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; + pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP]; + pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; + pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; + + // Set the roll attitude PI constants + pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + + // Set the pitch attitude PI constants + pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP]; + pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI]; + + // Set the yaw attitude PI constants + pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP]; + pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI]; + + // Need to store the vbar sensitivity + vbar_sensitivity[0] = settings.VbarSensitivity[0]; + vbar_sensitivity[1] = settings.VbarSensitivity[1]; + vbar_sensitivity[2] = settings.VbarSensitivity[2]; // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; @@ -460,6 +518,11 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) gyro_alpha = 0; // not trusting this to resolve to 0 else gyro_alpha = expf(-fakeDt / settings.GyroTau); + + // Compute time constant for vbar decay term based on a tau + vbar_decay = expf(-fakeDt / settings.VbarTau); + vbar_gyros_suppress = settings.VbarGyroSuppress; + vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE; } diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index bf3ae7b01..1217ea99f 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -215,7 +215,7 @@ static void objectUpdatedCb(UAVObjEvent * ev) // Get object data ObjectPersistenceGet(&objper); - int retval = -1; + int retval = 1; // Execute action if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) { if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) { @@ -242,6 +242,13 @@ static void objectUpdatedCb(UAVObjEvent * ev) } // Save selected instance retval = UAVObjSave(obj, objper.InstanceID); + + // Not sure why this is needed + vTaskDelay(10); + + // Verify saving worked + if (retval == 0) + retval = UAVObjLoad(obj, objper.InstanceID); } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) { retval = UAVObjSaveSettings(); @@ -271,9 +278,17 @@ static void objectUpdatedCb(UAVObjEvent * ev) retval = PIOS_FLASHFS_Format(); #endif } - if(retval == 0) { - objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&objper); + switch(retval) { + case 0: + objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&objper); + break; + case -1: + objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR; + ObjectPersistenceSet(&objper); + break; + default: + break; } } } diff --git a/flight/PiOS/Common/pios_flashfs_objlist.c b/flight/PiOS/Common/pios_flashfs_objlist.c index a11f6b14b..4111eb029 100644 --- a/flight/PiOS/Common/pios_flashfs_objlist.c +++ b/flight/PiOS/Common/pios_flashfs_objlist.c @@ -186,6 +186,7 @@ static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId) * @retval -2 No room in object table * @retval -3 Unable to write entry into object table * @retval -4 FS not initialized + * @retval -5 */ int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId) { @@ -206,6 +207,10 @@ int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId) if((addr + sizeof(header)) > cfg->obj_table_end) return -2; + // Verify the address is within the chip + if((addr + cfg->sector_size) > cfg->chip_size) + return -5; + if(PIOS_Flash_Jedec_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0) return -3; diff --git a/flight/PiOS/inc/pios_flashfs_objlist.h b/flight/PiOS/inc/pios_flashfs_objlist.h index 33b0bec96..a330da230 100644 --- a/flight/PiOS/inc/pios_flashfs_objlist.h +++ b/flight/PiOS/inc/pios_flashfs_objlist.h @@ -37,6 +37,7 @@ struct flashfs_cfg { uint32_t obj_table_start; uint32_t obj_table_end; uint32_t sector_size; + uint32_t chip_size; }; int32_t PIOS_FLASHFS_Init(const struct flashfs_cfg * cfg); diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 0d57da254..705463186 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -234,6 +234,7 @@ static const struct flashfs_cfg flashfs_m25p_cfg = { .obj_table_start = 0x00000010, .obj_table_end = 0x00010000, .sector_size = 0x00010000, + .chip_size = 0x00200000, }; static const struct pios_flash_jedec_cfg flash_m25p_cfg = { diff --git a/flight/UAVObjects/uavobjectmanager.c b/flight/UAVObjects/uavobjectmanager.c index d37b00312..8dc077967 100644 --- a/flight/UAVObjects/uavobjectmanager.c +++ b/flight/UAVObjects/uavobjectmanager.c @@ -721,10 +721,11 @@ int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId) return -1; // Fire event on success - if (PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data) == 0) + int32_t retval; + if ((retval = PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data)) == 0) sendEvent(objEntry, instId, EV_UNPACKED); else - return -1; + return retval; #endif #if defined(PIOS_INCLUDE_SDCARD) diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index 212621d44..e80986399 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -2,6 +2,7 @@ #include "ui_inputchannelform.h" #include "manualcontrolsettings.h" +#include "gcsreceiver.h" inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) : ConfigTaskWidget(parent), @@ -115,7 +116,7 @@ void inputChannelForm::groupUpdated() count = 18; break; case ManualControlSettings::CHANNELGROUPS_GCS: - count = 5; + count = GCSReceiver::CHANNEL_NUMELEM; break; case ManualControlSettings::CHANNELGROUPS_NONE: count = 0; diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp new file mode 100644 index 000000000..51daf2460 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp @@ -0,0 +1,468 @@ +#include "aerosimrc.h" +#include +#include +#include + +AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms) + : Simulator(params) +{ + udpCounterASrecv = 0; +} + +AeroSimRCSimulator::~AeroSimRCSimulator() +{ +} + +bool AeroSimRCSimulator::setupProcess() +{ + QMutexLocker locker(&lock); + return true; +} + +void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort) +{ + Q_UNUSED(outPort) + if (inSocket->bind(QHostAddress(host), inPort)) + emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n"); + else + emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n"); +} + +void AeroSimRCSimulator::transmitUpdate() +{ + // read actuator output + ActuatorCommand::DataFields actCmdData; + actCmdData = actCommand->getData(); + float channels[10]; + for (int i = 0; i < 10; ++i) { + qint16 ch = actCmdData.Channel[i]; + float out = -1.0; + if (ch >= 1000 && ch <= 2000) { + ch -= 1000; + out = ((float)ch / 500.0) - 1.0; + } + channels[i] = out; + } + + // read flight status + FlightStatus::DataFields flightData; + flightData = flightStatus->getData(); + quint8 armed; + quint8 mode; + armed = flightData.Armed; + mode = flightData.FlightMode; + + QByteArray data; + // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32) + data.resize(50); + QDataStream stream(&data, QIODevice::WriteOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); + stream << quint32(0x52434D44); // magic header, "RCMD" + for (int i = 0; i < 10; ++i) + stream << channels[i]; // channels + stream << armed << mode; // flight status + stream << udpCounterASrecv; // packet counter + + if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { + qDebug() << "write failed: " << outSocket->errorString(); + } + +#ifdef DBG_TIMERS + static int cntTX = 0; + if (cntTX >= 100) { + qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart(); + cntTX = 0; + } else { + ++cntTX; + } +#endif +} + +void AeroSimRCSimulator::processUpdate(const QByteArray &data) +{ + // check size + if (data.size() > 188) { + qDebug() << "!!! big datagram: " << data.size(); + return; + } + + QByteArray buf = data; + QDataStream stream(&buf, QIODevice::ReadOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); + + // check magic header + quint32 magic; + stream >> magic; + if (magic != 0x4153494D) { // "AERO" + qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D); + return; + } + + float timeStep, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, // world + velX, velY, velZ, // world + angX, angY, angZ, // model + accX, accY, accZ, // model + lat, lon, agl, // world + yaw, pitch, roll, // model + volt, curr, + rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix + ch[8]; + + stream >> timeStep; + stream >> homeX >> homeY >> homeZ; + stream >> WpHX >> WpHY >> WpLat >> WpLon; + stream >> posX >> posY >> posZ; + stream >> velX >> velY >> velZ; + stream >> angX >> angY >> angZ; + stream >> accX >> accY >> accZ; + stream >> lat >> lon >> agl; + stream >> yaw >> pitch >> roll; + stream >> volt >> curr; + stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz; + stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7]; + stream >> udpCounterASrecv; + + /**********************************************************************************************/ + QTime currentTime = QTime::currentTime(); + /**********************************************************************************************/ + static bool firstRun = true; + if (settings.homeLocation) { + if (firstRun) { + HomeLocation::DataFields homeData; + homeData = posHome->getData(); + + homeData.Latitude = WpLat * 10e6; + homeData.Longitude = WpLon * 10e6; + homeData.Altitude = homeZ; + homeData.Set = HomeLocation::SET_TRUE; + + posHome->setData(homeData); + + firstRun = false; + } + if (settings.homeLocRate > 0) { + static QTime homeLocTime = currentTime; + if (homeLocTime.secsTo(currentTime) >= settings.homeLocRate) { + firstRun = true; + homeLocTime = currentTime; + } + } + } + /**********************************************************************************************/ + if (settings.attRaw || settings.attActual) { + QMatrix4x4 mat; + mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix + ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); + mat.optimize(); + + QQuaternion quat; // model quat + asMatrix2Quat(mat, quat); + + // rotate gravity + QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) + QVector3D gee = QVector3D(0.0, 0.0, -GEE); + QQuaternion qWorld = quat.conjugate(); + gee = qWorld.rotatedVector(gee); + acc += gee; + + /*************************************************************************************/ + if (settings.attRaw) { + Accels::DataFields accelsData; + accelsData = accels->getData(); + Gyros::DataFields gyrosData; + gyrosData = gyros->getData(); + + gyrosData.x = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) + gyrosData.y = angX * RAD2DEG; + gyrosData.z = angZ * -RAD2DEG; + accelsData.x = acc.x(); + accelsData.y = acc.y(); + accelsData.z = acc.z(); + + accels->setData(accelsData); + gyros->setData(gyrosData); + } + /*************************************************************************************/ + if (settings.attActHW) { + // do nothing + /*****************************************/ + } else if (settings.attActSim) { + // take all data from simulator + AttitudeActual::DataFields attActData; + attActData = attActual->getData(); + + QVector3D rpy; // model roll, pitch, yaw + asMatrix2RPY(mat, rpy); + + attActData.Roll = rpy.x(); + attActData.Pitch = rpy.y(); + attActData.Yaw = rpy.z(); + attActData.q1 = quat.scalar(); + attActData.q2 = quat.x(); + attActData.q3 = quat.y(); + attActData.q4 = quat.z(); + + attActual->setData(attActData); + /*****************************************/ + } else if (settings.attActCalc) { + // calculate RPY with code from Attitude module + AttitudeActual::DataFields attActData; + attActData = attActual->getData(); + + static float q[4] = {1, 0, 0, 0}; + static float gyro_correct_int2 = 0; + + float dT = timeStep; + + AttitudeSettings::DataFields attSettData = attSettings->getData(); + float accelKp = attSettData.AccelKp * 0.1666666666666667; + float accelKi = attSettData.AccelKp * 0.1666666666666667; + float yawBiasRate = attSettData.YawBiasRate; + + // calibrate sensors on arming + if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) { + accelKp = 2.0; + accelKi = 0.9; + } + + float gyro[3] = {angY * RAD2DEG, angX * RAD2DEG, angZ * -RAD2DEG}; + float attRawAcc[3] = {acc.x(), acc.y(), acc.z()}; + + // code from Attitude module begin /////////////////////////////// + float *accels = attRawAcc; + float grot[3]; + float accel_err[3]; + + // Rotate gravity to body frame and cross with accels + grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + + // CrossProduct + { + accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; + accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; + accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; + } + + // Account for accel magnitude + float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]); + accel_err[0] /= accel_mag; + accel_err[1] /= accel_mag; + accel_err[2] /= accel_mag; + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyro_correct_int2 += -gyro[2] * yawBiasRate; + + // Correct rates based on error, integral component dealt with in updateSensors + gyro[0] += accel_err[0] * accelKp / dT; + gyro[1] += accel_err[1] * accelKp / dT; + gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2; + + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] += qdot[0]; + q[1] += qdot[1]; + q[2] += qdot[2]; + q[3] += qdot[3]; + + if(q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + + // Renomalize + float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + q[0] /= qmag; + q[1] /= qmag; + q[2] /= qmag; + q[3] /= qmag; + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if((fabs(qmag) < 1e-3) || (qmag != qmag)) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } + + float rpy2[3]; + // Quaternion2RPY + { + float q0s, q1s, q2s, q3s; + q0s = q[0] * q[0]; + q1s = q[1] * q[1]; + q2s = q[2] * q[2]; + q3s = q[3] * q[3]; + + float R13, R11, R12, R23, R33; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; + + rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy2[2] = RAD2DEG * atan2f(R12, R11); + rpy2[0] = RAD2DEG * atan2f(R23, R33); + } + + attActData.Roll = rpy2[0]; + attActData.Pitch = rpy2[1]; + attActData.Yaw = rpy2[2]; + attActData.q1 = q[0]; + attActData.q2 = q[1]; + attActData.q3 = q[2]; + attActData.q4 = q[3]; + attActual->setData(attActData); + /*****************************************/ + } + } + /**********************************************************************************************/ + if (settings.gcsReciever) { + static QTime gcsRcvrTime = currentTime; + if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) { + GCSReceiver::DataFields gcsRcvrData; + gcsRcvrData = gcsReceiver->getData(); + + for (int i = 0; i < 8; ++i) + gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500); + + gcsReceiver->setData(gcsRcvrData); + if (settings.manualOutput) + gcsRcvrTime = currentTime; + } + } else if (settings.manualControl) { + // not implemented yet + } + /**********************************************************************************************/ + if (settings.gpsPosition) { + static QTime gpsPosTime = currentTime; + if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { + GPSPosition::DataFields gpsPosData; + gpsPosData = gpsPosition->getData(); + + gpsPosData.Altitude = posZ; + gpsPosData.Heading = yaw * RAD2DEG; + gpsPosData.Latitude = lat * 10e6; + gpsPosData.Longitude = lon * 10e6; + gpsPosData.Groundspeed = qSqrt(velX * velX + velY * velY); + gpsPosData.GeoidSeparation = 0.0; + gpsPosData.Satellites = 8; + gpsPosData.PDOP = 3.0; + gpsPosData.Status = GPSPosition::STATUS_FIX3D; + + gpsPosition->setData(gpsPosData); + gpsPosTime = currentTime; + } + } + /**********************************************************************************************/ + if (settings.sonarAltitude) { + static QTime sonarAltTime = currentTime; + if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { + SonarAltitude::DataFields sonarAltData; + sonarAltData = sonarAlt->getData(); + + float sAlt = settings.sonarMaxAlt; + // 0.35 rad ~= 20 degree + if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { + float x = agl * qTan(roll); + float y = agl * qTan(pitch); + float h = qSqrt(x*x + y*y + agl*agl); + sAlt = qMin(h, sAlt); + } + + sonarAltData.Altitude = sAlt; + sonarAlt->setData(sonarAltData); + sonarAltTime = currentTime; + } + } + /**********************************************************************************************/ +/* + BaroAltitude::DataFields altActData; + altActData = altActual->getData(); + altActData.Altitude = posZ; + altActual->setData(altActData); + + PositionActual::DataFields posActData; + posActData = posActual->getData(); + posActData.North = posY * 100; + posActData.East = posX * 100; + posActData.Down = posZ * -100; + posActual->setData(posActData); + + VelocityActual::DataFields velActData; + velActData = velActual->getData(); + velActData.North = velY * 100; + velActData.East = velX * 100; + velActData.Down = velZ * 100; + velActual->setData(velActData); +*/ + +#ifdef DBG_TIMERS + static int cntRX = 0; + if (cntRX >= 100) { + qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart(); + cntRX = 0; + } else { + ++cntRX; + } +#endif +} + +// transfomations + +void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q) +{ + qreal w, x, y, z; + + // w always >= 0 + w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0; + x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0; + y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0; + z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0; + + x = copysign(x, (m(1, 2) - m(2, 1))); + y = copysign(y, (m(2, 0) - m(0, 2))); + z = copysign(z, (m(0, 1) - m(1, 0))); + + q.setScalar(w); + q.setX(x); + q.setY(y); + q.setZ(z); +} + +void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) +{ + qreal roll, pitch, yaw; + + if (qFabs(m(0, 2)) > 0.998) { + // ~86.3°, gimbal lock + roll = 0.0; + pitch = copysign(M_PI_2, -m(0, 2)); + yaw = qAtan2(-m(1, 0), m(1, 1)); + } else { + roll = qAtan2(m(1, 2), m(2, 2)); + pitch = qAsin(-m(0, 2)); + yaw = qAtan2(m(0, 1), m(0, 0)); + } + + rpy.setX(roll * RAD2DEG); + rpy.setY(pitch * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h new file mode 100644 index 000000000..19dae3bce --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h @@ -0,0 +1,46 @@ +#ifndef AEROSIMRC_H +#define AEROSIMRC_H + +#include +#include +#include +#include +#include "simulatorv2.h" + +class AeroSimRCSimulator: public Simulator +{ + Q_OBJECT + +public: + AeroSimRCSimulator(const SimulatorSettings ¶ms); + ~AeroSimRCSimulator(); + + bool setupProcess(); + void setupUdpPorts(const QString& host, int inPort, int outPort); + +private slots: + void transmitUpdate(); + +private: + quint32 udpCounterASrecv; //keeps track of udp packets received by ASim + + void processUpdate(const QByteArray &data); + + void asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q); + void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy); +}; + +class AeroSimRCSimulatorCreator : public SimulatorCreator +{ +public: + AeroSimRCSimulatorCreator(const QString &classId, const QString &description) + : SimulatorCreator (classId, description) + {} + + Simulator* createSimulator(const SimulatorSettings ¶ms) + { + return new AeroSimRCSimulator(params); + } +}; + +#endif // AEROSIMRC_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec new file mode 100644 index 000000000..26fede385 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec @@ -0,0 +1,12 @@ + + The OpenPilot Project + (C) 2011 OpenPilot Project + The GNU Public License (GPL) Version 3 + Hardware In The Loop Simulation (v2) + http://www.openpilot.org + + + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro new file mode 100644 index 000000000..f567fc401 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro @@ -0,0 +1,27 @@ +TEMPLATE = lib +TARGET = HITLV2 +QT += network +include(../../openpilotgcsplugin.pri) +include(hitlv2_dependencies.pri) +HEADERS += \ + aerosimrc.h \ + hitlv2configuration.h \ + hitlv2factory.h \ + hitlv2gadget.h \ + hitlv2optionspage.h \ + hitlv2plugin.h \ + hitlv2widget.h \ + simulatorv2.h +SOURCES += \ + aerosimrc.cpp \ + hitlv2configuration.cpp \ + hitlv2factory.cpp \ + hitlv2gadget.cpp \ + hitlv2optionspage.cpp \ + hitlv2plugin.cpp \ + hitlv2widget.cpp \ + simulatorv2.cpp +OTHER_FILES += hitlv2.pluginspec +FORMS += \ + hitlv2optionspage.ui \ + hitlv2widget.ui diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri new file mode 100644 index 000000000..50268face --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri @@ -0,0 +1,4 @@ +include(../../plugins/uavobjects/uavobjects.pri) +include(../../plugins/uavtalk/uavtalk.pri) +#include(../../plugins/coreplugin/coreplugin.pri) +#include(../../libs/utils/utils.pri) diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp new file mode 100644 index 000000000..0c4b42d39 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp @@ -0,0 +1,178 @@ +/** + ****************************************************************************** + * + * @file hitlconfiguration.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "hitlv2configuration.h" + +HITLConfiguration::HITLConfiguration(QString classId, + QSettings* qSettings, + QObject *parent) + : IUAVGadgetConfiguration(classId, parent) +{ +// qDebug() << "HITLV2Configuration"; + // default values + QString simulatorId = ""; + QString hostAddress = "127.0.0.1"; + int inPort = 40100; + QString remoteAddress = "127.0.0.1"; + int outPort = 40200; + QString binPath = ""; + QString dataPath = ""; + + bool homeLocation = true; + quint16 homeLocRate = 0; + + bool attRaw = true; + quint8 attRawRate = 20; + + bool attActual = true; + bool attActHW = false; + bool attActSim = true; + bool attActCalc = false; + + bool sonarAltitude = false; + float sonarMaxAlt = 5.0; + quint16 sonarAltRate = 50; + + bool gpsPosition = true; + quint16 gpsPosRate = 200; + + bool inputCommand = true; + bool gcsReciever = true; + bool manualControl = false; + + bool manualOutput = false; + quint8 outputRate = 20; + + // if a saved configuration exists load it + if (qSettings != 0) { + settings.simulatorId = qSettings->value("simulatorId", simulatorId).toString(); + settings.hostAddress = qSettings->value("hostAddress", hostAddress).toString(); + settings.inPort = qSettings->value("inPort", inPort).toInt(); + settings.remoteAddress = qSettings->value("remoteAddress", remoteAddress).toString(); + settings.outPort = qSettings->value("outPort", outPort).toInt(); + settings.binPath = qSettings->value("binPath", binPath).toString(); + settings.dataPath = qSettings->value("dataPath", dataPath).toString(); + + settings.homeLocation = qSettings->value("homeLocation", homeLocation).toBool(); + settings.homeLocRate = qSettings->value("homeLocRate", homeLocRate).toInt(); + + settings.attRaw = qSettings->value("attRaw", attRaw).toBool(); + settings.attRawRate = qSettings->value("attRawRate", attRawRate).toInt(); + + settings.attActual = qSettings->value("attActual", attActual).toBool(); + settings.attActHW = qSettings->value("attActHW", attActHW).toBool(); + settings.attActSim = qSettings->value("attActSim", attActSim).toBool(); + settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool(); + + settings.sonarAltitude = qSettings->value("sonarAltitude", sonarAltitude).toBool(); + settings.sonarMaxAlt = qSettings->value("sonarMaxAlt", sonarMaxAlt).toFloat(); + settings.sonarAltRate = qSettings->value("sonarAltRate", sonarAltRate).toInt(); + + settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt(); + + settings.inputCommand = qSettings->value("inputCommand", inputCommand).toBool(); + settings.gcsReciever = qSettings->value("gcsReciever", gcsReciever).toBool(); + settings.manualControl = qSettings->value("manualControl", manualControl).toBool(); + settings.manualOutput = qSettings->value("manualOutput", manualOutput).toBool(); + settings.outputRate = qSettings->value("outputRate", outputRate).toInt(); + } else { + settings.simulatorId = simulatorId; + settings.hostAddress = hostAddress; + settings.inPort = inPort; + settings.remoteAddress = remoteAddress; + settings.outPort = outPort; + settings.binPath = binPath; + settings.dataPath = dataPath; + + settings.homeLocation = homeLocation; + settings.homeLocRate = homeLocRate; + + settings.attRaw = attRaw; + settings.attRawRate = attRawRate; + + settings.attActual = attActual; + settings.attActHW = attActHW; + settings.attActSim = attActSim; + settings.attActCalc = attActCalc; + + settings.sonarAltitude = sonarAltitude; + settings.sonarMaxAlt = sonarMaxAlt; + settings.sonarAltRate = sonarAltRate; + + settings.gpsPosition = gpsPosition; + settings.gpsPosRate = gpsPosRate; + + settings.inputCommand = inputCommand; + settings.gcsReciever = gcsReciever; + settings.manualControl = manualControl; + settings.manualOutput = manualOutput; + settings.outputRate = outputRate; + } +} + +IUAVGadgetConfiguration *HITLConfiguration::clone() +{ + HITLConfiguration *m = new HITLConfiguration(this->classId()); + m->settings = settings; + return m; +} + +void HITLConfiguration::saveConfig(QSettings* qSettings) const +{ + qSettings->setValue("simulatorId", settings.simulatorId); + qSettings->setValue("hostAddress", settings.hostAddress); + qSettings->setValue("inPort", settings.inPort); + qSettings->setValue("remoteAddress", settings.remoteAddress); + qSettings->setValue("outPort", settings.outPort); + qSettings->setValue("binPath", settings.binPath); + qSettings->setValue("dataPath", settings.dataPath); + + qSettings->setValue("homeLocation", settings.homeLocation); + qSettings->setValue("homeLocRate", settings.homeLocRate); + + qSettings->setValue("attRaw", settings.attRaw); + qSettings->setValue("attRawRate", settings.attRawRate); + + qSettings->setValue("attActual", settings.attActual); + qSettings->setValue("attActHW", settings.attActHW); + qSettings->setValue("attActSim", settings.attActSim); + qSettings->setValue("attActCalc", settings.attActCalc); + + qSettings->setValue("sonarAltitude", settings.sonarAltitude); + qSettings->setValue("sonarMaxAlt", settings.sonarMaxAlt); + qSettings->setValue("sonarAltRate", settings.sonarAltRate); + + qSettings->setValue("gpsPosition", settings.gpsPosition); + qSettings->setValue("gpsPosRate", settings.gpsPosRate); + + qSettings->setValue("inputCommand", settings.inputCommand); + qSettings->setValue("gcsReciever", settings.gcsReciever); + qSettings->setValue("manualControl", settings.manualControl); + qSettings->setValue("manualOutput", settings.manualOutput); + qSettings->setValue("outputRate", settings.outputRate); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h new file mode 100644 index 000000000..dfac80ddf --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file hitlconfiguration.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 HITLV2CONFIGURATION_H +#define HITLV2CONFIGURATION_H + +#include +#include +#include +#include + +using namespace Core; + +class HITLConfiguration : public IUAVGadgetConfiguration +{ + + Q_OBJECT + + Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) + +public: + explicit HITLConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + + void saveConfig(QSettings* settings) const; + IUAVGadgetConfiguration *clone(); + + SimulatorSettings Settings() const { return settings; } + +public slots: + void setSimulatorSettings (const SimulatorSettings& params ) { settings = params; } + + +private: + SimulatorSettings settings; +}; + +#endif // HITLV2CONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp new file mode 100644 index 000000000..6e6f1a42e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp @@ -0,0 +1,58 @@ +/** + ****************************************************************************** + * + * @file hitlfactory.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "hitlv2factory.h" +#include "hitlv2widget.h" +#include "hitlv2gadget.h" +#include "hitlv2configuration.h" +#include "hitlv2optionspage.h" +#include + +HITLFactory::HITLFactory(QObject *parent) + : IUAVGadgetFactory(QString("HITLv2"), tr("HITL Simulation (v2)"), parent) +{ +} + +HITLFactory::~HITLFactory() +{ +} + +Core::IUAVGadget* HITLFactory::createGadget(QWidget *parent) +{ + HITLWidget* gadgetWidget = new HITLWidget(parent); + return new HITLGadget(QString("HITLv2"), gadgetWidget, parent); +} + +IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings* qSettings) +{ + return new HITLConfiguration(QString("HITLv2"), qSettings); +} + +IOptionsPage *HITLFactory::createOptionsPage(IUAVGadgetConfiguration *config) +{ + return new HITLOptionsPage(qobject_cast(config)); +} + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h new file mode 100644 index 000000000..b4142eeed --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * + * @file hitlfactory.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 HITLV2FACTORY_H +#define HITLV2FACTORY_H + +#include + +namespace Core { +class IUAVGadget; +class IUAVGadgetFactory; +} + +using namespace Core; + +class HITLFactory : public Core::IUAVGadgetFactory +{ + Q_OBJECT +public: + HITLFactory(QObject *parent = 0); + ~HITLFactory(); + + IUAVGadget *createGadget(QWidget *parent); + IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); +}; + +#endif // HITLV2FACTORY_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp new file mode 100644 index 000000000..de22d121b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file hitl.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "hitlv2gadget.h" +#include "hitlv2widget.h" +#include "hitlv2configuration.h" +#include "simulatorv2.h" + +HITLGadget::HITLGadget(QString classId, HITLWidget *widget, QWidget *parent) : + IUAVGadget(classId, parent), + m_widget(widget) +{ + connect(this, SIGNAL(changeConfiguration(void)), m_widget, SLOT(stopButtonClicked(void))); +} + +HITLGadget::~HITLGadget() +{ + delete m_widget; +} + +void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config) +{ + HITLConfiguration *m = qobject_cast(config); + emit changeConfiguration(); + m_widget->setSettingParameters(m->Settings()); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h new file mode 100644 index 000000000..0e4fd1a80 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * + * @file hitl.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 HITLV2_H +#define HITLV2_H + +#include +#include "hitlv2widget.h" + +class IUAVGadget; +class QWidget; +class QString; +class Simulator; + +using namespace Core; + +class HITLGadget : public Core::IUAVGadget +{ + Q_OBJECT +public: + HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); + ~HITLGadget(); + + QWidget *widget() { return m_widget; } + void loadConfiguration(IUAVGadgetConfiguration* config); + +signals: + void changeConfiguration(); + +private: + HITLWidget* m_widget; + Simulator* simulator; +}; + + +#endif // HITLV2_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp new file mode 100644 index 000000000..5b88785ad --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp @@ -0,0 +1,146 @@ +/** + ****************************************************************************** + * + * @file hitloptionspage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "hitlv2optionspage.h" +#include "hitlv2configuration.h" +#include "ui_hitlv2optionspage.h" +#include "hitlv2plugin.h" + +#include +#include +#include +#include "simulatorv2.h" + +HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : + IOptionsPage(parent), + config(conf) +{ +} + +QWidget *HITLOptionsPage::createPage(QWidget *parent) +{ +// qDebug() << "HITLOptionsPage::createPage"; + // Create page + m_optionsPage = new Ui::HITLOptionsPage(); + QWidget* optionsPageWidget = new QWidget; + m_optionsPage->setupUi(optionsPageWidget); + int index = 0; + foreach (SimulatorCreator* creator, HITLPlugin::typeSimulators) + m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(), creator->ClassId()); + + m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); + m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); + m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); + m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); + + // Restore the contents from the settings: + foreach (SimulatorCreator* creator, HITLPlugin::typeSimulators) { + QString id = config->Settings().simulatorId; + if (creator->ClassId() == id) + m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); + } + + m_optionsPage->hostAddress->setText(config->Settings().hostAddress); + m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); + m_optionsPage->remoteAddress->setText(config->Settings().remoteAddress); + m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); + m_optionsPage->executablePath->setPath(config->Settings().binPath); + m_optionsPage->dataPath->setPath(config->Settings().dataPath); + + m_optionsPage->homeLocation->setChecked(config->Settings().homeLocation); + m_optionsPage->homeLocRate->setValue(config->Settings().homeLocRate); + + m_optionsPage->attRaw->setChecked(config->Settings().attRaw); + m_optionsPage->attRawRate->setValue(config->Settings().attRawRate); + + m_optionsPage->attActual->setChecked(config->Settings().attActual); + m_optionsPage->attActHW->setChecked(config->Settings().attActHW); + m_optionsPage->attActSim->setChecked(config->Settings().attActSim); + m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc); + + m_optionsPage->sonarAltitude->setChecked(config->Settings().sonarAltitude); + m_optionsPage->sonarMaxAlt->setValue(config->Settings().sonarMaxAlt); + m_optionsPage->sonarAltRate->setValue(config->Settings().sonarAltRate); + + m_optionsPage->gpsPosition->setChecked(config->Settings().gpsPosition); + m_optionsPage->gpsPosRate->setValue(config->Settings().gpsPosRate); + + m_optionsPage->inputCommand->setChecked(config->Settings().inputCommand); + m_optionsPage->gcsReciever->setChecked(config->Settings().gcsReciever); + m_optionsPage->manualControl->setChecked(config->Settings().manualControl); + + m_optionsPage->manualOutput->setChecked(config->Settings().manualOutput); + m_optionsPage->outputRate->setValue(config->Settings().outputRate); + + return optionsPageWidget; +} + +void HITLOptionsPage::apply() +{ + SimulatorSettings settings; + int i = m_optionsPage->chooseFlightSimulator->currentIndex(); + + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.inPort = m_optionsPage->inputPort->text().toInt(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + + settings.homeLocation = m_optionsPage->homeLocation->isChecked(); + settings.homeLocRate = m_optionsPage->homeLocRate->value(); + + settings.attRaw = m_optionsPage->attRaw->isChecked(); + settings.attRawRate = m_optionsPage->attRawRate->value(); + + settings.attActual = m_optionsPage->attActual->isChecked(); + settings.attActHW = m_optionsPage->attActHW->isChecked(); + settings.attActSim = m_optionsPage->attActSim->isChecked(); + settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + + settings.sonarAltitude = m_optionsPage->sonarAltitude->isChecked(); + settings.sonarMaxAlt = m_optionsPage->sonarMaxAlt->value(); + settings.sonarAltRate = m_optionsPage->sonarAltRate->value(); + + settings.gpsPosition = m_optionsPage->gpsPosition->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRate->value(); + + settings.inputCommand = m_optionsPage->inputCommand->isChecked(); + settings.gcsReciever = m_optionsPage->gcsReciever->isChecked(); + settings.manualControl = m_optionsPage->manualControl->isChecked(); + + settings.manualOutput = m_optionsPage->manualOutput->isChecked(); + settings.outputRate = m_optionsPage->outputRate->value(); + + config->setSimulatorSettings(settings); +} + +void HITLOptionsPage::finish() +{ + delete m_optionsPage; +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h new file mode 100644 index 000000000..5c8d33f59 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file hitloptionspage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 HITLV2OPTIONSPAGE_H +#define HITLV2OPTIONSPAGE_H + +#include + +namespace Core { +class IUAVGadgetConfiguration; +} + +class HITLConfiguration; + +using namespace Core; + +namespace Ui { +class HITLOptionsPage; +} + +class HITLOptionsPage : public IOptionsPage +{ + Q_OBJECT +public: + explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); + + QWidget *createPage(QWidget *parent); + void apply(); + void finish(); + bool isDecorated() const { return true; } + +private: + HITLConfiguration* config; + Ui::HITLOptionsPage* m_optionsPage; +}; + +#endif // HITLV2OPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui new file mode 100644 index 000000000..00a2f60aa --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui @@ -0,0 +1,639 @@ + + + HITLOptionsPage + + + + 0 + 0 + 403 + 400 + + + + Form + + + + 3 + + + 0 + + + 0 + + + 0 + + + 3 + + + + + + + Choose flight simulator: + + + + + + + + + + + + + + Local interface (IP): + + + + + + + For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. + + + + + + + Remote interface (IP): + + + + + + + Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. + + + + + + + Input Port: + + + + + + + For receiving data from sim + + + + + + + Output Port: + + + + + + + For sending data to sim + + + + + + + + + QFormLayout::AllNonFixedFieldsGrow + + + + + + 0 + 0 + + + + Path executable: + + + + + + + + + + + 0 + 0 + + + + Data directory: + + + + + + + + + + + + + + Attitude data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + AttitudeRaw (gyro, accels) + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + ms + + + 10 + + + 100 + + + 20 + + + + + + + + + + AttitudeActual + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + send raw data to board + + + + + + + + 75 + true + + + + + + + use values from simulator + + + true + + + + + + + + + + calculate from AttitudeRaw + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Other data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + HomeLocation + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + 0 - update once, or every N seconds + + + sec + + + 10 + + + + + + + + + + GPSPosition + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + ms + + + 100 + + + 2000 + + + 500 + + + + + + + + + + SonarAltitude + + + true + + + true + + + false + + + + 3 + + + 6 + + + 3 + + + 0 + + + 0 + + + + + + + Range detectioon + + + + + + + m + + + 1 + + + 10 + + + 5 + + + + + + + Refresh rate + + + + + + + ms + + + 20 + + + 2000 + + + 10 + + + 50 + + + + + + + + + + + + Map command from simulator + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + + 75 + true + + + + to GCSReciver + + + true + + + + + + + false + + + to ManualCtrll (not implemented) + + + + + + + + + + 6 + + + + + Maximum output rate + + + + + + + ms + + + 10 + + + 100 + + + 5 + + + 15 + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Utils::PathChooser + QWidget +
utils/pathchooser.h
+ 1 + + + + chooseFlightSimulator + hostAddress + inputPort + remoteAddress + outputPort + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp new file mode 100644 index 000000000..f0ee2d07a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp @@ -0,0 +1,70 @@ +/** + ****************************************************************************** + * + * @file mapplugin.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "hitlv2plugin.h" +#include "hitlv2factory.h" +#include +#include +#include + +#include "aerosimrc.h" + +QList HITLPlugin::typeSimulators; + +HITLPlugin::HITLPlugin() +{ + // Do nothing +} + +HITLPlugin::~HITLPlugin() +{ + // Do nothing +} + +bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new HITLFactory(this); + + addAutoReleasedObject(mf); + + addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); + + return true; +} + +void HITLPlugin::extensionsInitialized() +{ + // Do nothing +} + +void HITLPlugin::shutdown() +{ + // Do nothing +} +Q_EXPORT_PLUGIN(HITLPlugin) + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h new file mode 100644 index 000000000..e68129bd4 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h @@ -0,0 +1,67 @@ +/** + ****************************************************************************** + * + * @file browserplugin.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 HITLV2PLUGIN_H +#define HITLV2PLUGIN_H + +#include +#include + +#include + +class HITLFactory; + +class HITLPlugin : public ExtensionSystem::IPlugin +{ +public: + HITLPlugin(); + ~HITLPlugin(); + + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString * errorString); + void shutdown(); + + static void addSimulator(SimulatorCreator* creator) + { + HITLPlugin::typeSimulators.append(creator); + } + + static SimulatorCreator* getSimulatorCreator(const QString classId) + { + foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) { + if(classId == creator->ClassId()) + return creator; + } + return 0; + } + + static QList typeSimulators; + +private: + HITLFactory *mf; +}; +#endif /* HITLV2PLUGIN_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp new file mode 100644 index 000000000..55d14f735 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp @@ -0,0 +1,187 @@ +/** + ****************************************************************************** + * + * @file hitlwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "hitlv2widget.h" +#include "ui_hitlv2widget.h" +#include +#include +#include +#include +#include + +#include "hitlv2plugin.h" +#include "simulatorv2.h" +#include "uavobjectmanager.h" +#include +#include + +QStringList Simulator::instances; + +HITLWidget::HITLWidget(QWidget *parent) + : QWidget(parent), + simulator(0) +{ + widget = new Ui_HITLWidget(); + widget->setupUi(this); + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); + + strAutopilotDisconnected = " AP OFF "; + strSimulatorDisconnected = " Sim OFF "; + strAutopilotConnected = " AP ON "; +// strSimulatorConnected = " Sim ON "; + strStyleEnable = "QFrame{background-color: green; color: white}"; + strStyleDisable = "QFrame{background-color: red; color: grey}"; + + widget->apLabel->setText(strAutopilotDisconnected); + widget->simLabel->setText(strSimulatorDisconnected); + + connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); + connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); + connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); +} + +HITLWidget::~HITLWidget() +{ + delete widget; +} + +void HITLWidget::startButtonClicked() +{ + // allow only one instance per simulator + if (Simulator::Instances().indexOf(settings.simulatorId) != -1) { + widget->textBrowser->append(settings.simulatorId + " alreary started!"); + return; + } + + if (!HITLPlugin::typeSimulators.size()) { + widget->textBrowser->append("There is no registered simulators, add through HITLPlugin::addSimulator"); + return; + } + + // Stop running process if one is active + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } + + if (settings.hostAddress == "" || settings.inPort == 0) { + widget->textBrowser->append("Before start, set UDP parameters in options page!"); + return; + } + + SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); + simulator = creator->createSimulator(settings); + simulator->setName(creator->Description()); + simulator->setSimulatorId(creator->ClassId()); + + connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString))); + + // Setup process + onProcessOutput(QString("[%1] Starting %2... ") + .arg(QTime::currentTime().toString("hh:mm:ss")) + .arg(creator->Description())); + + // Start bridge + bool ret = QMetaObject::invokeMethod(simulator, "setupProcess", Qt::QueuedConnection); + if (ret) { + Simulator::setInstance(settings.simulatorId); + + connect(this, SIGNAL(deleteSimulator()), simulator, SLOT(onDeleteSimulator()), Qt::QueuedConnection); + + widget->startButton->setEnabled(false); + widget->stopButton->setEnabled(true); + + connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()), Qt::QueuedConnection); + + // Initialize connection status + if (simulator->isAutopilotConnected()) + onAutopilotConnect(); + else + onAutopilotDisconnect(); + + if (simulator->isSimulatorConnected()) + onSimulatorConnect(); + else + onSimulatorDisconnect(); + } +} + +void HITLWidget::stopButtonClicked() +{ + if (simulator) + widget->textBrowser->append(QString("[%1] Terminate %2 ") + .arg(QTime::currentTime().toString("hh:mm:ss")) + .arg(simulator->Name())); + + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); + widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); + widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); + widget->apLabel->setText(strAutopilotDisconnected); + widget->simLabel->setText(strSimulatorDisconnected); + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } +} + +void HITLWidget::buttonClearLogClicked() +{ + widget->textBrowser->clear(); +} + +void HITLWidget::onProcessOutput(QString text) +{ + widget->textBrowser->append(text); +} + +void HITLWidget::onAutopilotConnect() +{ + widget->apLabel->setStyleSheet(strStyleEnable); + widget->apLabel->setText(strAutopilotConnected); +} + +void HITLWidget::onAutopilotDisconnect() +{ + widget->apLabel->setStyleSheet(strStyleDisable); + widget->apLabel->setText(strAutopilotDisconnected); +} + +void HITLWidget::onSimulatorConnect() +{ + widget->simLabel->setStyleSheet(strStyleEnable); + widget->simLabel->setText(" " + simulator->Name() + " ON "); +} + +void HITLWidget::onSimulatorDisconnect() +{ + widget->simLabel->setStyleSheet(strStyleDisable); + widget->simLabel->setText(" " + simulator->Name() + " OFF "); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h new file mode 100644 index 000000000..78f3e9405 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h @@ -0,0 +1,72 @@ +/** + ****************************************************************************** + * + * @file hitlwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 HITLV2WIDGET_H +#define HITLV2WIDGET_H + +#include +#include +#include "simulatorv2.h" + +class Ui_HITLWidget; + +class HITLWidget : public QWidget +{ + Q_OBJECT + +public: + HITLWidget(QWidget *parent = 0); + ~HITLWidget(); + + void setSettingParameters(const SimulatorSettings& params) { settings = params; } + +signals: + void deleteSimulator(); + +private slots: + void startButtonClicked(); + void stopButtonClicked(); + void buttonClearLogClicked(); + void onProcessOutput(QString text); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onSimulatorConnect(); + void onSimulatorDisconnect(); + +private: + Ui_HITLWidget* widget; + Simulator* simulator; + SimulatorSettings settings; + + QString strAutopilotDisconnected; + QString strSimulatorDisconnected; + QString strAutopilotConnected; +// QString strSimulatorConnected; + QString strStyleEnable; + QString strStyleDisable; +}; +#endif /* HITLV2WIDGET_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui new file mode 100644 index 000000000..48d2c2db2 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui @@ -0,0 +1,314 @@ + + + HITLWidget + + + + 0 + 0 + 477 + 300 + + + + + 0 + 0 + + + + Form + + + + +QScrollBar:vertical { + border: 1px solid grey; + background: grey; + margin: 22px 0 22px 0; + } + +QScrollBar:vertical:disabled { + border: 1px solid grey; + + background-color: grey; + margin: 22px 0 22px 0; + } + + QScrollBar::handle:vertical { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(120, 120, 255, 255), stop:1 rgba(80, 80, 160, 255)); + min-height: 20px; + } + +QScrollBar::handle:vertical:disabled{ +background-color: grey; + min-height: 20px; + } + + + QScrollBar::handle:vertical:pressed { + + background-color: rgb(85, 85, 255); +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(170, 170, 255, 255), stop:1 rgba(80, 80, 160, 255)); + + min-height: 20px; + } + + QScrollBar::add-line:vertical { + border: 1px solid black; +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); + height: 20px; + subcontrol-position: bottom; + subcontrol-origin: margin; + } + + QScrollBar::add-line:vertical:disabled { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(100, 100, 100, 255)); + border: 1px solid grey; + + } + + QScrollBar::sub-line:vertical:disabled { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(100, 100, 100, 255)); + border: 1px solid grey; + + } + + QScrollBar::add-line:vertical:pressed { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); + } + + QScrollBar::sub-line:vertical:pressed { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); + } + + QScrollBar::sub-line:vertical { + border: 1px solid black; +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); + height: 20px; + subcontrol-position: top; + subcontrol-origin: margin; + } + QScrollBar::down-arrow:vertical { + + image: url(:/hitlnew/images/arrow-down2.png); + } + + QScrollBar::up-arrow:vertical { + image: url(:/hitlnew/images/arrow-up2.png); + } + + QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical { + background: none; + } + + +QPushButton { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(110, 110, 110, 255), stop:1 rgba(71, 71, 71, 255)); + + color: rgb(255, 255, 255); +border: 1px solid black; +width: 66px; +height: 20px; +} + +QPushButton:disabled { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(120, 120, 120, 255)); + color: rgb(194, 194, 194); +border: 1px solid gray; +width: 66px; +height: 20px; +} + +QPushButton:hover { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); +color: rgb(255, 255, 255); +border: 0px; +} +QPushButton:pressed { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); +color: rgb(255, 255, 255); +border: 0px; +} + +QPushButton:checked { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); +color: rgb(255, 255, 255); +border: 0px; +} + + + + 0 + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + +QFrame{ +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(110, 110, 110, 255), stop:1 rgba(71, 71, 71, 255)); +color: rgba(0, 0, 0, 128); +} + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + QLayout::SetMaximumSize + + + + + QLayout::SetDefaultConstraint + + + + + Request update + + + + + + Start + + + + + + + false + + + Send update + + + Stop + + + + + + + + 0 + 22 + + + + + 50 + false + + + + Qt::LeftToRight + + + false + + + color: rgb(255, 255, 255); + + + AP OFF + + + Qt::AlignCenter + + + + + + + + 0 + 22 + + + + color: rgb(255, 255, 255); + + + Sim OFF + + + Qt::AlignCenter + + + + + + + Clear Log + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + false + + + QTextEdit { + background-color: white; + color: rgb(0, 0, 0); +} + + + QFrame::NoFrame + + + QFrame::Plain + + + Qt::ScrollBarAlwaysOn + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp new file mode 100644 index 000000000..c4c698689 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp @@ -0,0 +1,341 @@ +/** + ****************************************************************************** + * + * @file simulator.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 "simulatorv2.h" +#include +#include +#include + +volatile bool Simulator::isStarted = false; + +const float Simulator::GEE = 9.81; +const float Simulator::FT2M = 0.3048; +const float Simulator::KT2MPS = 0.514444444; +const float Simulator::INHG2KPA = 3.386; +const float Simulator::FPS2CMPS = 30.48; +const float Simulator::DEG2RAD = (M_PI/180.0); +const float Simulator::RAD2DEG = (180.0/M_PI); + + +Simulator::Simulator(const SimulatorSettings& params) : + inSocket(NULL), + outSocket(NULL), + settings(params), + updatePeriod(50), + simTimeout(2000), + autopilotConnectionStatus(false), + simConnectionStatus(false), + txTimer(NULL), + simTimer(NULL), + name("") +{ + // move to thread + moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); + connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection); + emit myStart(); +} + +Simulator::~Simulator() +{ +// qDebug() << "Simulator::~Simulator"; + if (inSocket) { + delete inSocket; + inSocket = NULL; + } + if (outSocket) { + delete outSocket; + outSocket = NULL; + } + if (txTimer) { + delete txTimer; + txTimer = NULL; + } + if (simTimer) { + delete simTimer; + simTimer = NULL; + } +} + +void Simulator::onDeleteSimulator(void) +{ +// qDebug() << "Simulator::onDeleteSimulator"; + resetAllObjects(); + + Simulator::setStarted(false); + Simulator::Instances().removeOne(simulatorId); + + disconnect(this); + delete this; +} + +void Simulator::onStart() +{ +// qDebug() << "Simulator::onStart"; + QMutexLocker locker(&lock); + + // Get required UAVObjects + ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager* objManager = pm->getObject(); + +// actDesired = ActuatorDesired::GetInstance(objManager); +// manCtrlCommand = ManualControlCommand::GetInstance(objManager); +// velActual = VelocityActual::GetInstance(objManager); +// posActual = PositionActual::GetInstance(objManager); +// altActual = BaroAltitude::GetInstance(objManager); +// camDesired = CameraDesired::GetInstance(objManager); +// acsDesired = AccessoryDesired::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + accels = Accels::GetInstance(objManager); + gyros = Gyros::GetInstance(objManager); + attActual = AttitudeActual::GetInstance(objManager); + gpsPosition = GPSPosition::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); + gcsReceiver = GCSReceiver::GetInstance(objManager); + actCommand = ActuatorCommand::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + sonarAlt = SonarAltitude::GetInstance(objManager); + + telStats = GCSTelemetryStats::GetInstance(objManager); + + // Listen to autopilot connection events + TelemetryManager* telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + + // If already connect setup autopilot + GCSTelemetryStats::DataFields stats = telStats->getData(); + if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) + onAutopilotConnect(); + + emit processOutput("Local interface: " + settings.hostAddress + ":" + \ + QString::number(settings.inPort) + "\n" + \ + "Remote interface: " + settings.remoteAddress + ":" + \ + QString::number(settings.outPort) + "\n"); + + inSocket = new QUdpSocket(); + outSocket = new QUdpSocket(); + setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort); + + connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate())/*, Qt::DirectConnection*/); + + // Setup transmit timer + if (settings.manualOutput) { + txTimer = new QTimer(); + connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate())/*, Qt::DirectConnection*/); + txTimer->setInterval(settings.outputRate); + txTimer->start(); + } + + // Setup simulator connection timer + simTimer = new QTimer(); + connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout())/*, Qt::DirectConnection*/); + simTimer->setInterval(simTimeout); + simTimer->start(); + +#ifdef DBG_TIMERS + timeRX = QTime(); + timeRX.start(); + timeTX = QTime(); + timeTX.start(); +#endif + + setupObjects(); +} + +void Simulator::receiveUpdate() +{ + // Update connection timer and status + simTimer->start(); + if (!simConnectionStatus) { + simConnectionStatus = true; + emit simulatorConnected(); + } + + // Process data + while (inSocket->hasPendingDatagrams()) { + // Receive datagram + QByteArray datagram; + datagram.resize(inSocket->pendingDatagramSize()); + QHostAddress sender; + quint16 senderPort; + inSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); + // Process incomming data + processUpdate(datagram); + } + if (!settings.manualOutput) + transmitUpdate(); +} + +void Simulator::setupObjects() +{ + if (settings.gcsReciever) { + setupInputObject(actCommand, settings.outputRate); + setupOutputObject(gcsReceiver); + } else if (settings.manualControl) { +// setupInputObject(actDesired); +// setupInputObject(camDesired); +// setupInputObject(acsDesired); +// setupOutputObject(manCtrlCommand); + qDebug() << "ManualControlCommand not implemented yet"; + } + + if (settings.homeLocation) + setupOutputObject(posHome); + + if (settings.sonarAltitude) + setupOutputObject(sonarAlt); + + if (settings.gpsPosition) + setupOutputObject(gpsPosition); + + if (settings.attRaw || settings.attActual) { + setupOutputObject(accels); + setupOutputObject(gyros); + } + + if (settings.attActual && !settings.attActHW) + setupOutputObject(attActual); + else + setupWatchedObject(attActual); +} + +void Simulator::resetAllObjects() +{ + setupDefaultObject(posHome); + setupDefaultObject(accels); + setupDefaultObject(gyros); + setupDefaultObject(attActual); + setupDefaultObject(gpsPosition); + setupDefaultObject(gcsReceiver); + setupDefaultObject(actCommand); + setupDefaultObject(sonarAlt); +// setupDefaultObject(manCtrlCommand); +// setupDefaultObject(actDesired); +// setupDefaultObject(camDesired); +// setupDefaultObject(acsDesired); +// setupDefaultObject(altActual); +// setupDefaultObject(posActual); +// setupDefaultObject(velActual); +} + +void Simulator::setupInputObject(UAVObject* obj, quint32 updateRate) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); + UAVObject::SetGcsTelemetryAcked(mdata, false); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); + mdata.gcsTelemetryUpdatePeriod = 0; + + UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); + UAVObject::SetFlightTelemetryAcked(mdata, false); + + if (settings.manualOutput) { + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = updateRate; + } else { + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); + mdata.flightTelemetryUpdatePeriod = 0; + } + + obj->setMetadata(mdata); +} + +void Simulator::setupWatchedObject(UAVObject *obj) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); + UAVObject::SetGcsTelemetryAcked(mdata, false); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); + mdata.gcsTelemetryUpdatePeriod = 0; + + UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); + UAVObject::SetFlightTelemetryAcked(mdata, false); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 100; + + obj->setMetadata(mdata); +} + +void Simulator::setupOutputObject(UAVObject* obj) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); + UAVObject::SetGcsTelemetryAcked(mdata, false); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); + mdata.gcsTelemetryUpdatePeriod = 0; + + UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); + UAVObject::SetFlightTelemetryAcked(mdata, false); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); + mdata.flightTelemetryUpdatePeriod = 0; + + obj->setMetadata(mdata); +} + +void Simulator::setupDefaultObject(UAVObject *obj) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + obj->setMetadata(mdata); +} + +void Simulator::onAutopilotConnect() +{ + autopilotConnectionStatus = true; + emit autopilotConnected(); +} + +void Simulator::onAutopilotDisconnect() +{ + autopilotConnectionStatus = false; + emit autopilotDisconnected(); +} + +void Simulator::onSimulatorConnectionTimeout() +{ + if (simConnectionStatus) { + simConnectionStatus = false; + emit simulatorDisconnected(); + } +} + +void Simulator::telStatsUpdated(UAVObject* obj) +{ + GCSTelemetryStats::DataFields stats = telStats->getData(); + if (!autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED) + onAutopilotConnect(); + else if (autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED) + onAutopilotDisconnect(); +} + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h new file mode 100644 index 000000000..6a0a9f008 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h @@ -0,0 +1,222 @@ +/** + ****************************************************************************** + * + * @file simulator.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop 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 ISIMULATORV2_H +#define ISIMULATORV2_H + +#include +#include +#include +#include +#include +#include +#include "uavtalk/telemetrymanager.h" +#include "uavobjectmanager.h" +#include "homelocation.h" +#include "accels.h" +#include "gyros.h" +#include "attitudeactual.h" +#include "gpsposition.h" +#include "flightstatus.h" +#include "gcsreceiver.h" +#include "actuatorcommand.h" +#include "gcstelemetrystats.h" +#include "attitudesettings.h" +#include "sonaraltitude.h" + +//#define DBG_TIMERS +#undef DBG_TIMERS + +/** + * just imagine this was a class without methods and all public properties + */ + +typedef struct _CONNECTION +{ + QString simulatorId; + QString hostAddress; + int inPort; + QString remoteAddress; + int outPort; + QString binPath; + QString dataPath; + + bool homeLocation; + quint16 homeLocRate; + + bool attRaw; + quint8 attRawRate; + + bool attActual; + bool attActHW; + bool attActSim; + bool attActCalc; + + bool sonarAltitude; + float sonarMaxAlt; + quint16 sonarAltRate; + + bool gpsPosition; + quint16 gpsPosRate; + + bool inputCommand; + bool gcsReciever; + bool manualControl; + bool manualOutput; + quint8 outputRate; + +} SimulatorSettings; + +class Simulator : public QObject +{ + Q_OBJECT + +public: + Simulator(const SimulatorSettings& params); + virtual ~Simulator(); + + bool isAutopilotConnected() const { return autopilotConnectionStatus; } + bool isSimulatorConnected() const { return simConnectionStatus; } + + QString Name() const { return name; } + void setName(QString str) { name = str; } + + QString SimulatorId() const { return simulatorId; } + void setSimulatorId(QString str) { simulatorId = str; } + + static bool IsStarted() { return isStarted; } + static void setStarted(bool val) { isStarted = val; } + + static QStringList& Instances() { return Simulator::instances; } + static void setInstance(const QString& str) { Simulator::instances.append(str); } + + virtual void stopProcess() {} + virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} + +signals: + void processOutput(QString str); + void autopilotConnected(); + void autopilotDisconnected(); + void simulatorConnected(); + void simulatorDisconnected(); + void myStart(); + +public slots: + Q_INVOKABLE virtual bool setupProcess() { return true; } + +private slots: + void onStart(); + void receiveUpdate(); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onSimulatorConnectionTimeout(); + void telStatsUpdated(UAVObject* obj); + Q_INVOKABLE void onDeleteSimulator(void); + + virtual void transmitUpdate() = 0; + virtual void processUpdate(const QByteArray& data) = 0; + +protected: + static const float GEE; + static const float FT2M; + static const float KT2MPS; + static const float INHG2KPA; + static const float FPS2CMPS; + static const float DEG2RAD; + static const float RAD2DEG; + +#ifdef DBG_TIMERS + QTime timeRX; + QTime timeTX; +#endif + + QUdpSocket* inSocket; + QUdpSocket* outSocket; + +// ActuatorDesired* actDesired; +// ManualControlCommand* manCtrlCommand; +// VelocityActual* velActual; +// PositionActual* posActual; +// BaroAltitude* altActual; +// CameraDesired *camDesired; +// AccessoryDesired *acsDesired; + Accels *accels; + Gyros *gyros; + AttitudeActual *attActual; + HomeLocation *posHome; + FlightStatus *flightStatus; + GPSPosition *gpsPosition; + GCSReceiver *gcsReceiver; + ActuatorCommand *actCommand; + AttitudeSettings *attSettings; + SonarAltitude *sonarAlt; + + GCSTelemetryStats* telStats; + SimulatorSettings settings; + + QMutex lock; + +private: + int updatePeriod; + int simTimeout; + volatile bool autopilotConnectionStatus; + volatile bool simConnectionStatus; + QTimer* txTimer; + QTimer* simTimer; + QString name; + QString simulatorId; + volatile static bool isStarted; + static QStringList instances; + + void setupObjects(); + void resetAllObjects(); + void setupInputObject(UAVObject* obj, quint32 updateRate); + void setupOutputObject(UAVObject* obj); + void setupWatchedObject(UAVObject *obj); + void setupDefaultObject(UAVObject *obj); +}; + +class SimulatorCreator +{ +public: + SimulatorCreator(QString id, QString descr) : + classId(id), + description(descr) + {} + virtual ~SimulatorCreator() {} + + QString ClassId() const { return classId; } + QString Description() const { return description; } + + virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; + +private: + QString classId; + QString description; +}; + +#endif // ISIMULATORV2_H diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index fad1b0c3a..154335e3a 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -147,6 +147,13 @@ plugin_hitlnew.depends += plugin_uavobjects plugin_hitlnew.depends += plugin_uavtalk SUBDIRS += plugin_hitlnew +#HITLNEW Simulation gadget v2 +plugin_hitl_v2.subdir = hitlv2 +plugin_hitl_v2.depends = plugin_coreplugin +plugin_hitl_v2.depends += plugin_uavobjects +plugin_hitl_v2.depends += plugin_uavtalk +SUBDIRS += plugin_hitl_v2 + # Export and Import GCS Configuration plugin_importexport.subdir = importexport plugin_importexport.depends = plugin_coreplugin diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 1d1d208a9..a30172fb0 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -35,6 +35,7 @@ #include #include #include +#include // ****************************** // constructor/destructor @@ -199,25 +200,63 @@ void UAVObjectUtilManager::objectPersistenceOperationFailed() */ void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject * obj) { - qDebug() << "objectPersistenceUpdated: " << obj->getField("Operation")->getValue().toString(); - Q_ASSERT(obj->getName().compare("ObjectPersistence") == 0); - if(saveState == AWAITING_COMPLETED) { - failureTimer.stop(); - // Check flight is saying it completed. This is the only thing flight should do to trigger an update. - Q_ASSERT( obj->getField("Operation")->getValue().toString().compare(QString("Completed")) == 0 ); + Q_ASSERT(obj); + Q_ASSERT(obj->getObjID() == ObjectPersistence::OBJID); + ObjectPersistence::DataFields objectPersistence = ((ObjectPersistence *)obj)->getData(); + if(saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) { + failureTimer.stop(); + objectPersistenceOperationFailed(); + } else if (saveState == AWAITING_COMPLETED && + objectPersistence.Operation == ObjectPersistence::OPERATION_COMPLETED) { + failureTimer.stop(); // Check right object saved UAVObject* savingObj = queue.head(); - Q_ASSERT( obj->getField("ObjectID")->getValue() == savingObj->getObjID() ); + if(objectPersistence.ObjectID != savingObj->getObjID() ) { + objectPersistenceOperationFailed(); + return; + } obj->disconnect(this); queue.dequeue(); // We can now remove the object, it's done. saveState = IDLE; - emit saveCompleted(obj->getField("ObjectID")->getValue().toInt(), true); + + emit saveCompleted(objectPersistence.ObjectID, true); saveNextObject(); } } +/** + * Helper function that makes sure FirmwareIAP is updated and then returns the data + */ +FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap() +{ + FirmwareIAPObj::DataFields dummy; + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); + if (!pm) + return dummy; + UAVObjectManager *om = pm->getObject(); + Q_ASSERT(om); + if (!om) + return dummy; + + FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(om); + Q_ASSERT(firmwareIap); + if (!firmwareIap) + return dummy; + + // The code below will ask for the object update and wait for the updated to be received, + // or the timeout of the timer, set to 1 second. + QEventLoop loop; + connect(firmwareIap, SIGNAL(objectUpdated(UAVObject*)), &loop, SLOT(quit())); + QTimer::singleShot(1000, &loop, SLOT(quit())); // Create a timeout + firmwareIap->requestUpdate(); + loop.exec(); + + return firmwareIap->getData(); +} /** * Get the UAV Board model, for anyone interested. Return format is: @@ -225,25 +264,8 @@ void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject * obj) */ int UAVObjectUtilManager::getBoardModel() { - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) - return 0; - UAVObjectManager *om = pm->getObject(); - if (!om) - return 0; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("FirmwareIAPObj"))); - // The code below will ask for the object update and wait for the updated to be received, - // or the timeout of the timer, set to 1 second. - QEventLoop loop; - connect(obj, SIGNAL(objectUpdated(UAVObject*)), &loop, SLOT(quit())); - QTimer::singleShot(1000, &loop, SLOT(quit())); // Create a timeout - obj->requestUpdate(); - loop.exec(); - - int boardType = (obj->getField("BoardType")->getValue().toInt()) << 8; - boardType += obj->getField("BoardRevision")->getValue().toInt(); - return boardType; + FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); + return (firmwareIapData.BoardType << 8) + firmwareIapData.BoardRevision; } /** @@ -252,54 +274,18 @@ int UAVObjectUtilManager::getBoardModel() QByteArray UAVObjectUtilManager::getBoardCPUSerial() { QByteArray cpuSerial; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) - return 0; - UAVObjectManager *om = pm->getObject(); - if (!om) - return 0; + FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - UAVDataObject *obj = dynamic_cast(om->getObject(QString("FirmwareIAPObj"))); - // The code below will ask for the object update and wait for the updated to be received, - // or the timeout of the timer, set to 1 second. - QEventLoop loop; - connect(obj, SIGNAL(objectUpdated(UAVObject*)), &loop, SLOT(quit())); - QTimer::singleShot(1000, &loop, SLOT(quit())); // Create a timeout - obj->requestUpdate(); - loop.exec(); + for (int i = 0; i < FirmwareIAPObj::CPUSERIAL_NUMELEM; i++) + cpuSerial.append(firmwareIapData.CPUSerial[i]); - UAVObjectField* cpuField = obj->getField("CPUSerial"); - for (uint i = 0; i < cpuField->getNumElements(); ++i) { - cpuSerial.append(cpuField->getValue(i).toUInt()); - } return cpuSerial; } quint32 UAVObjectUtilManager::getFirmwareCRC() { - quint32 fwCRC; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) - return 0; - UAVObjectManager *om = pm->getObject(); - if (!om) - return 0; - - UAVDataObject *obj = dynamic_cast(om->getObject(QString("FirmwareIAPObj"))); - obj->getField("crc")->setValue(0); - obj->updated(); - // The code below will ask for the object update and wait for the updated to be received, - // or the timeout of the timer, set to 1 second. - QEventLoop loop; - connect(obj, SIGNAL(objectUpdated(UAVObject*)), &loop, SLOT(quit())); - QTimer::singleShot(1000, &loop, SLOT(quit())); // Create a timeout - obj->requestUpdate(); - loop.exec(); - - UAVObjectField* fwCRCField = obj->getField("crc"); - - fwCRC=(quint32)fwCRCField->getValue().toLongLong(); - return fwCRC; + FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); + return firmwareIapData.crc; } /** @@ -308,27 +294,11 @@ quint32 UAVObjectUtilManager::getFirmwareCRC() QByteArray UAVObjectUtilManager::getBoardDescription() { QByteArray ret; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (!pm) - return 0; - UAVObjectManager *om = pm->getObject(); - if (!om) - return 0; + FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - UAVDataObject *obj = dynamic_cast(om->getObject(QString("FirmwareIAPObj"))); - // The code below will ask for the object update and wait for the updated to be received, - // or the timeout of the timer, set to 1 second. - QEventLoop loop; - connect(obj, SIGNAL(objectUpdated(UAVObject*)), &loop, SLOT(quit())); - QTimer::singleShot(1000, &loop, SLOT(quit())); // Create a timeout - obj->requestUpdate(); - loop.exec(); + for (int i = 0; i < FirmwareIAPObj::DESCRIPTION_NUMELEM; i++) + ret.append(firmwareIapData.Description[i]); - UAVObjectField* descriptionField = obj->getField("Description"); - // Description starts with an offset of - for (uint i = 0; i < descriptionField->getNumElements(); ++i) { - ret.append(descriptionField->getValue(i).toInt()); - } return ret; } diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 6c7782b25..6f71123b8 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -43,6 +43,8 @@ #include #include #include +#include + class UAVOBJECTUTIL_EXPORT UAVObjectUtilManager: public QObject { Q_OBJECT @@ -69,6 +71,8 @@ public: static bool descriptionToStructure(QByteArray desc,deviceDescriptorStruct * struc); UAVObjectManager* getObjectManager(); void saveObjectToSD(UAVObject *obj); +protected: + FirmwareIAPObj::DataFields getFirmwareIap(); signals: void saveCompleted(int objectID, bool status); diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml index 9e48e6381..cbc00c965 100644 --- a/shared/uavobjectdefinition/gcsreceiver.xml +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -1,8 +1,8 @@ A receiver channel group carried over the telemetry link. - - + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 8ed79dbd0..c2c8ec715 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -18,9 +18,9 @@ - - - + + + diff --git a/shared/uavobjectdefinition/objectpersistence.xml b/shared/uavobjectdefinition/objectpersistence.xml index afeb2ff24..2d73e07b3 100644 --- a/shared/uavobjectdefinition/objectpersistence.xml +++ b/shared/uavobjectdefinition/objectpersistence.xml @@ -1,7 +1,7 @@ Someone who knows please enter this - + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 21138c2e3..02c63cd90 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index de1c92daa..7cd7de210 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -13,6 +13,15 @@ + + + + + + + + +