diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index d90010a51..89fe6fd44 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -235,6 +235,7 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_bmp085.c +SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_iap.c #SRC += $(PIOSCOMMON)/pios_opahrs.c #SRC += $(PIOSCOMMON)/pios_opahrs_proto.c diff --git a/flight/OpenPilot/Modules/Actuator/actuator.c b/flight/OpenPilot/Modules/Actuator/actuator.c index 841d670fe..c6114aae1 100644 --- a/flight/OpenPilot/Modules/Actuator/actuator.c +++ b/flight/OpenPilot/Modules/Actuator/actuator.c @@ -67,6 +67,8 @@ static void actuatorTask(void* parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static void setFailsafe(); static float MixerCurve(const float throttle, const float* curve); +static bool set_channel(uint8_t mixer_channel, uint16_t value); + float ProcessMixer(const int index, const float curve1, const float curve2, MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period); @@ -99,7 +101,6 @@ int32_t ActuatorInitialize() } - /** * @brieft Main Actuator module task * @@ -201,25 +202,42 @@ static void actuatorTask(void* parameters) lastResult[ct] = 0; }else { + // For motors when armed keep above neutral + if((mixer[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) && (status[ct] < 0)) + status[ct] = 0; + command.Channel[ct] = scaleChannel(status[ct], settings.ChannelMax[ct], - settings.ChannelMin[ct], + settings.ChannelNeutral[ct], settings.ChannelNeutral[ct]); } } } MixerStatusSet(&mixerStatus); + // Store update time + command.UpdateTime = 10000*dT; + if(command.UpdateTime > command.MaxUpdateTime) + command.MaxUpdateTime = command.UpdateTime; + // Update output object ActuatorCommandSet(&command); // Update in case read only (eg. during servo configuration) ActuatorCommandGet(&command); // Update servo outputs + bool success = true; for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - PIOS_Servo_Set( n, command.Channel[n] ); + success &= set_channel(n, command.Channel[n]); } + + if(!success) { + command.NumFailedUpdates++; + ActuatorCommandSet(&command); + AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); + } + } } @@ -385,13 +403,47 @@ static void setFailsafe() // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - PIOS_Servo_Set( n, command.Channel[n] ); + set_channel(n, command.Channel[n]); } // Update output object ActuatorCommandSet(&command); } +static bool set_channel(uint8_t mixer_channel, uint16_t value) { + + ActuatorSettingsData settings; + ActuatorSettingsGet(&settings); + + switch(settings.ChannelType[mixer_channel]) { + case ACTUATORSETTINGS_CHANNELTYPE_PWM: + PIOS_Servo_Set(settings.ChannelAddr[mixer_channel], value); + return true; + case ACTUATORSETTINGS_CHANNELTYPE_MK: + { + ManualControlCommandData manual; + ManualControlCommandGet(&manual); + /* Unfortunately MK controller take forever to start so keep */ + /* them spinning when armed */ + if(manual.Armed) + value = (value < 0) ? 1 : value; + else + value = 0; + + return PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value); + break; + } + case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: + return PIOS_SetAstec4Speed(settings.ChannelAddr[mixer_channel],value); + break; + default: + return false; + } + + return false; + +} + /** * @} diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 482895145..33e462ba4 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -58,7 +58,7 @@ #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_WDG - +#define PIOS_INCLUDE_I2C_ESC /* Defaults for Logging */ #define LOG_FILENAME "PIOS.LOG" diff --git a/flight/OpenPilot/UAVObjects/actuatorsettings.c b/flight/OpenPilot/UAVObjects/actuatorsettings.c index e733f1871..08ca8c08c 100644 --- a/flight/OpenPilot/UAVObjects/actuatorsettings.c +++ b/flight/OpenPilot/UAVObjects/actuatorsettings.c @@ -120,6 +120,22 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) data.ChannelMin[5] = 1000; data.ChannelMin[6] = 1000; data.ChannelMin[7] = 1000; + data.ChannelType[0] = 0; + data.ChannelType[1] = 0; + data.ChannelType[2] = 0; + data.ChannelType[3] = 0; + data.ChannelType[4] = 0; + data.ChannelType[5] = 0; + data.ChannelType[6] = 0; + data.ChannelType[7] = 0; + data.ChannelAddr[0] = 0; + data.ChannelAddr[1] = 1; + data.ChannelAddr[2] = 2; + data.ChannelAddr[3] = 3; + data.ChannelAddr[4] = 4; + data.ChannelAddr[5] = 5; + data.ChannelAddr[6] = 6; + data.ChannelAddr[7] = 7; UAVObjSetInstanceData(obj, instId, &data); diff --git a/flight/OpenPilot/UAVObjects/inc/actuatorcommand.h b/flight/OpenPilot/UAVObjects/inc/actuatorcommand.h index 2d05dfecd..0d9eb28e3 100644 --- a/flight/OpenPilot/UAVObjects/inc/actuatorcommand.h +++ b/flight/OpenPilot/UAVObjects/inc/actuatorcommand.h @@ -41,7 +41,7 @@ #define ACTUATORCOMMAND_H // Object constants -#define ACTUATORCOMMAND_OBJID 3909877022U +#define ACTUATORCOMMAND_OBJID 3907024856U #define ACTUATORCOMMAND_NAME "ActuatorCommand" #define ACTUATORCOMMAND_METANAME "ActuatorCommandMeta" #define ACTUATORCOMMAND_ISSINGLEINST 1 @@ -72,6 +72,9 @@ // Object data typedef struct { int16_t Channel[8]; + uint8_t UpdateTime; + uint8_t MaxUpdateTime; + uint8_t NumFailedUpdates; } __attribute__((packed)) ActuatorCommandData; @@ -79,6 +82,9 @@ typedef struct { // Field Channel information /* Number of elements for field Channel */ #define ACTUATORCOMMAND_CHANNEL_NUMELEM 8 +// Field UpdateTime information +// Field MaxUpdateTime information +// Field NumFailedUpdates information // Generic interface functions diff --git a/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h b/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h index dd5e1b682..cb4e717cb 100644 --- a/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h @@ -41,7 +41,7 @@ #define ACTUATORSETTINGS_H // Object constants -#define ACTUATORSETTINGS_OBJID 3054509114U +#define ACTUATORSETTINGS_OBJID 844831578U #define ACTUATORSETTINGS_NAME "ActuatorSettings" #define ACTUATORSETTINGS_METANAME "ActuatorSettingsMeta" #define ACTUATORSETTINGS_ISSINGLEINST 1 @@ -89,6 +89,8 @@ typedef struct { int16_t ChannelMax[8]; int16_t ChannelNeutral[8]; int16_t ChannelMin[8]; + uint8_t ChannelType[8]; + uint8_t ChannelAddr[8]; } __attribute__((packed)) ActuatorSettingsData; @@ -147,6 +149,14 @@ typedef enum { ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOT // Field ChannelMin information /* Number of elements for field ChannelMin */ #define ACTUATORSETTINGS_CHANNELMIN_NUMELEM 8 +// Field ChannelType information +/* Enumeration options for field ChannelType */ +typedef enum { ACTUATORSETTINGS_CHANNELTYPE_PWM=0, ACTUATORSETTINGS_CHANNELTYPE_MK=1, ACTUATORSETTINGS_CHANNELTYPE_ASTEC4=2 } ActuatorSettingsChannelTypeOptions; +/* Number of elements for field ChannelType */ +#define ACTUATORSETTINGS_CHANNELTYPE_NUMELEM 8 +// Field ChannelAddr information +/* Number of elements for field ChannelAddr */ +#define ACTUATORSETTINGS_CHANNELADDR_NUMELEM 8 // Generic interface functions diff --git a/flight/PiOS/Common/pios_i2c_esc.c b/flight/PiOS/Common/pios_i2c_esc.c new file mode 100644 index 000000000..fdb58eba0 --- /dev/null +++ b/flight/PiOS/Common/pios_i2c_esc.c @@ -0,0 +1,172 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_I2C_ESC Code for controlling I2C based ESCs + * @brief Deals with the hardware interface to the magnetometers + * @{ + * + * @file pios_i2c_esc.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief HMC5843 Magnetic Sensor Functions from AHRS + * @see The GNU Public License (GPL) Version 3 + * + ****************************************************************************** + */ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_I2C_ESC) + +/* HMC5843 Addresses */ +#define MK_I2C_ADDR 0x29 +#define ASTEC4_I2C_ADDR 0x02 + +bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed); + +uint8_t base_address = MK_I2C_ADDR; +uint8_t valid_motors = 0; +bool PIOS_I2C_ESC_Config() +{ + base_address = MK_I2C_ADDR; + valid_motors = 0; + for(uint8_t i = 0; i < 4; i ++) + if(PIOS_SetMKSpeed(i, 0)) + valid_motors |= (1 << i); + + return true; +} + +bool PIOS_I2C_ESC_SetSpeed(uint8_t speed[4]) +{ + /*bool success = true; + for(uint8_t i = 0; i < 4; i++) { + //if(valid_motors & (1 << i)) + success &= PIOS_SetMKSpeed(i, speed[i]); + } + return success; */ + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MK_I2C_ADDR + 0, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[0]), + .buf = &speed[0], + } , + { + .info = __func__, + .addr = MK_I2C_ADDR + 1, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[1]), + .buf = &speed[1], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 2, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[2]), + .buf = &speed[2], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 3, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[3]), + .buf = &speed[3], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + +bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) { + static uint8_t speeds[4] = {0,0,0,0}; + + if(motornum >= 4) + return false; + + if(speeds[motornum] == speed) + return true; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MK_I2C_ADDR + motornum, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed), + .buf = &speed, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + +bool PIOS_SetAstec4Address(uint8_t new_address) { + if((new_address < 0) || (new_address > 4)) + return false; + + uint8_t data[4] = {250, 0, new_address, 230+new_address}; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ASTEC4_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = &data[0], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + +bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) { + static uint8_t speeds[5] = {0,0,0,0}; + + if((motornum < 0) || (motornum >= 4)) + return false; + + speeds[motornum] = speed; + + if(motornum != 3) + return true; + + /* Write in chunks of four */ + speeds[4] = 0xAA + speeds[0] + speeds[1] + speeds[2] + speeds[3]; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ASTEC4_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speeds), + .buf = &speeds[0], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + +#endif + +/** + * @} + * @} + */ diff --git a/flight/PiOS/STM32F10x/pios_i2c.c b/flight/PiOS/STM32F10x/pios_i2c.c index a16ec811e..10c8e91c8 100644 --- a/flight/PiOS/STM32F10x/pios_i2c.c +++ b/flight/PiOS/STM32F10x/pios_i2c.c @@ -353,7 +353,6 @@ static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter) #endif /* Note that this transfer has hit a bus error */ i2c_adapter->bus_error = true; -// i2c_adapter->curr_state = I2C_STATE_STOPPED; i2c_adapter_reset_bus(i2c_adapter); @@ -672,7 +671,7 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ /* ESC */ - bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; + //bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; while(GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { /* Set clock high and wait for any clock stretching to finish. */ @@ -688,17 +687,14 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); PIOS_DELAY_WaituS(2); } - if(sda_hung) { - /* Generate a start then stop condition */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - - } + /* Generate a start then stop condition */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); /* Set data and clock high and wait for any clock stretching to finish. */ GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); @@ -914,6 +910,8 @@ void PIOS_I2C_EV_IRQ_Handler(uint8_t i2c) i2c_adapter = find_i2c_adapter_by_id(i2c); PIOS_DEBUG_Assert(i2c_adapter); + PIOS_DELAY_WaituS(1); + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); #if defined(PIOS_I2C_DIAGNOSTICS) @@ -983,7 +981,7 @@ void PIOS_I2C_EV_IRQ_Handler(uint8_t i2c) while (halt) ; } break; - case 0: /* Unexplained spurious event. Not sure what to do here. */ + case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ @@ -1011,6 +1009,7 @@ void PIOS_I2C_EV_IRQ_Handler(uint8_t i2c) break; case 0x30084: /* Occurs between byte tranmistted and master mode selected */ case 0x30000: /* Need to throw away this spurious event */ + //case 0x0: /* Not sure why zeros are occurring */ goto skip_event; break; default: @@ -1026,25 +1025,50 @@ skip_event: ; } +uint32_t i2c_interrupt_history[5]; +uint8_t i2c_interrupt_history_pointer = 0; + void PIOS_I2C_ER_IRQ_Handler(uint8_t i2c) { struct pios_i2c_adapter *i2c_adapter; i2c_adapter = find_i2c_adapter_by_id(i2c); PIOS_DEBUG_Assert(i2c_adapter); + + PIOS_DELAY_WaituS(1); #if defined(PIOS_I2C_DIAGNOSTICS) uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); - /* Store event for diagnostics */ - i2c_event_history[i2c_event_history_pointer] = event; - i2c_event_history_pointer = (i2c_event_history_pointer + 1) % I2C_LOG_DEPTH; -#endif + i2c_interrupt_history[i2c_interrupt_history_pointer] = event; + i2c_interrupt_history_pointer = (i2c_interrupt_history_pointer + 1) % 5; - if(I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_AF)) { +#endif + + switch(event) { + case 0x70184: /* EV8_2, but system detects own stop byte as erroneous */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); + break; + } + return; + break; + } + + if(event & I2C_FLAG_AF) { i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); - } else { + } else { /* Mostly bus errors here */ i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); /* Fail hard on any errors for now */ diff --git a/flight/PiOS/inc/pios_i2c_esc.h b/flight/PiOS/inc/pios_i2c_esc.h new file mode 100644 index 000000000..c403030b3 --- /dev/null +++ b/flight/PiOS/inc/pios_i2c_esc.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_I2C_ESC Code for controlling I2C based ESCs + * @brief Deals with the hardware interface to the magnetometers + * @{ + * + * @file pios_i2c_esc.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief HMC5843 Magnetic Sensor Functions from AHRS + * @see The GNU Public License (GPL) Version 3 + * + ****************************************************************************** + */ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_I2C_ESC_H +#define PIOS_I2C_ESC_H + +/* Public Functions */ +#include +#include + +bool PIOS_I2C_ESC_Config(); +bool PIOS_I2C_ESC_SetSpeed(uint8_t speed[4]); +bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed); +bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed); + +#endif /* PIOS_I2C_ESC_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index f310b095c..ab01ca5b6 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -92,6 +92,9 @@ #if defined(PIOS_INCLUDE_HMC5843) #include #endif +#if defined(PIOS_INCLUDE_I2C_ESC) +#include +#endif #include #if defined(PIOS_INCLUDE_BL_HELPER) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index e72a3a245..04d6cb292 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -41,9 +41,6 @@ 6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = ""; }; 6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; - 655DBA5212DA5B6A00341B9A /* pios_iap.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_iap.h; sourceTree = ""; }; - 655DBA5312DA5E9900341B9A /* watchdogstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = watchdogstatus.xml; sourceTree = ""; }; - 655DBA8A12DA644300341B9A /* taskmonitor.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = taskmonitor.c; sourceTree = ""; }; 65632CBF124E09D900469B77 /* guidance.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = guidance.c; sourceTree = ""; }; 65632CC1124E09D900469B77 /* guidance.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = guidance.h; sourceTree = ""; }; 65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; @@ -6786,7 +6783,6 @@ 65B367FD121C2620003EAD18 /* systemstats.xml */, 65B367FE121C2620003EAD18 /* telemetrysettings.xml */, 65408AA812BB1648004DACC5 /* i2cstats.xml */, - 655DBA5312DA5E9900341B9A /* watchdogstatus.xml */, ); path = uavobjectdefinition; sourceTree = ""; @@ -7113,7 +7109,6 @@ 65E8EF5B11EEA61E00BBF654 /* System */ = { isa = PBXGroup; children = ( - 655DBA8A12DA644300341B9A /* taskmonitor.c */, 65E8EF5C11EEA61E00BBF654 /* alarms.c */, 65E8EF5D11EEA61E00BBF654 /* inc */, 65E8EF6511EEA61E00BBF654 /* openpilot.c */, @@ -7298,7 +7293,6 @@ 65E8F03811EFF25C00BBF654 /* inc */ = { isa = PBXGroup; children = ( - 655DBA5212DA5B6A00341B9A /* pios_iap.h */, 6526645A122DF972006F9A3C /* pios_i2c_priv.h */, 6526645B122DF972006F9A3C /* pios_wdg.h */, 651CF9EF120B700D00EEFD70 /* pios_usb_hid_desc.h */, diff --git a/ground/src/plugins/uavobjects/OPLogConvert.m b/ground/src/plugins/uavobjects/OPLogConvert.m index da4eaf53f..971494b3e 100644 --- a/ground/src/plugins/uavobjects/OPLogConvert.m +++ b/ground/src/plugins/uavobjects/OPLogConvert.m @@ -4,6 +4,9 @@ function [] = OPLogConvert() actuatorcommandIdx = 1; ActuatorCommand.timestamp = 0; ActuatorCommand(1).Channel = zeros(1,8); + ActuatorCommand(1).UpdateTime = 0; + ActuatorCommand(1).MaxUpdateTime = 0; + ActuatorCommand(1).NumFailedUpdates = 0; actuatordesiredIdx = 1; ActuatorDesired.timestamp = 0; @@ -34,6 +37,8 @@ function [] = OPLogConvert() ActuatorSettings(1).ChannelMax = zeros(1,8); ActuatorSettings(1).ChannelNeutral = zeros(1,8); ActuatorSettings(1).ChannelMin = zeros(1,8); + ActuatorSettings(1).ChannelType = zeros(1,8); + ActuatorSettings(1).ChannelAddr = zeros(1,8); ahrscalibrationIdx = 1; AHRSCalibration.timestamp = 0; @@ -453,13 +458,13 @@ function [] = OPLogConvert() %% Read object switch objID - case 3909877022 + case 3907024856 ActuatorCommand(actuatorcommandIdx) = ReadActuatorCommandObject(fid, timestamp); actuatorcommandIdx = actuatorcommandIdx + 1; case 3562104706 ActuatorDesired(actuatordesiredIdx) = ReadActuatorDesiredObject(fid, timestamp); actuatordesiredIdx = actuatordesiredIdx + 1; - case 3054509114 + case 844831578 ActuatorSettings(actuatorsettingsIdx) = ReadActuatorSettingsObject(fid, timestamp); actuatorsettingsIdx = actuatorsettingsIdx + 1; case 806362034 @@ -611,6 +616,9 @@ function [ActuatorCommand] = ReadActuatorCommandObject(fid, timestamp) ActuatorCommand.timestamp = timestamp; ActuatorCommand.Channel = double(fread(fid, 8, 'int16')); + ActuatorCommand.UpdateTime = double(fread(fid, 1, 'uint8')); + ActuatorCommand.MaxUpdateTime = double(fread(fid, 1, 'uint8')); + ActuatorCommand.NumFailedUpdates = double(fread(fid, 1, 'uint8')); % read CRC fread(fid, 1, 'uint8'); end @@ -661,6 +669,8 @@ function [ActuatorSettings] = ReadActuatorSettingsObject(fid, timestamp) ActuatorSettings.ChannelMax = double(fread(fid, 8, 'int16')); ActuatorSettings.ChannelNeutral = double(fread(fid, 8, 'int16')); ActuatorSettings.ChannelMin = double(fread(fid, 8, 'int16')); + ActuatorSettings.ChannelType = double(fread(fid, 8, 'uint8')); + ActuatorSettings.ChannelAddr = double(fread(fid, 8, 'uint8')); % read CRC fread(fid, 1, 'uint8'); end diff --git a/ground/src/plugins/uavobjects/actuatorcommand.cpp b/ground/src/plugins/uavobjects/actuatorcommand.cpp index 7f0c6bdeb..7aeb6d880 100644 --- a/ground/src/plugins/uavobjects/actuatorcommand.cpp +++ b/ground/src/plugins/uavobjects/actuatorcommand.cpp @@ -53,6 +53,15 @@ ActuatorCommand::ActuatorCommand(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING ChannelElemNames.append("6"); ChannelElemNames.append("7"); fields.append( new UAVObjectField(QString("Channel"), QString("us"), UAVObjectField::INT16, ChannelElemNames, QStringList()) ); + QStringList UpdateTimeElemNames; + UpdateTimeElemNames.append("0"); + fields.append( new UAVObjectField(QString("UpdateTime"), QString("ms x 10"), UAVObjectField::UINT8, UpdateTimeElemNames, QStringList()) ); + QStringList MaxUpdateTimeElemNames; + MaxUpdateTimeElemNames.append("0"); + fields.append( new UAVObjectField(QString("MaxUpdateTime"), QString("ms x 10"), UAVObjectField::UINT8, MaxUpdateTimeElemNames, QStringList()) ); + QStringList NumFailedUpdatesElemNames; + NumFailedUpdatesElemNames.append("0"); + fields.append( new UAVObjectField(QString("NumFailedUpdates"), QString(""), UAVObjectField::UINT8, NumFailedUpdatesElemNames, QStringList()) ); // Initialize object initializeFields(fields, (quint8*)&data, NUMBYTES); diff --git a/ground/src/plugins/uavobjects/actuatorcommand.h b/ground/src/plugins/uavobjects/actuatorcommand.h index b28e5dbb1..621838b74 100644 --- a/ground/src/plugins/uavobjects/actuatorcommand.h +++ b/ground/src/plugins/uavobjects/actuatorcommand.h @@ -44,6 +44,9 @@ public: // Field structure typedef struct { qint16 Channel[8]; + quint8 UpdateTime; + quint8 MaxUpdateTime; + quint8 NumFailedUpdates; } __attribute__((packed)) DataFields; @@ -51,10 +54,13 @@ public: // Field Channel information /* Number of elements for field Channel */ static const quint32 CHANNEL_NUMELEM = 8; + // Field UpdateTime information + // Field MaxUpdateTime information + // Field NumFailedUpdates information // Constants - static const quint32 OBJID = 3909877022U; + static const quint32 OBJID = 3907024856U; static const QString NAME; static const QString DESCRIPTION; static const bool ISSINGLEINST = 1; diff --git a/ground/src/plugins/uavobjects/actuatorcommand.py b/ground/src/plugins/uavobjects/actuatorcommand.py index f2e67df12..3b9b546c0 100644 --- a/ground/src/plugins/uavobjects/actuatorcommand.py +++ b/ground/src/plugins/uavobjects/actuatorcommand.py @@ -54,12 +54,42 @@ _fields = [ \ { } ), + uavobject.UAVObjectField( + 'UpdateTime', + 'B', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'MaxUpdateTime', + 'B', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'NumFailedUpdates', + 'B', + 1, + [ + '0', + ], + { + } + ), ] class ActuatorCommand(uavobject.UAVObject): ## Object constants - OBJID = 3909877022 + OBJID = 3907024856 NAME = "ActuatorCommand" METANAME = "ActuatorCommandMeta" ISSINGLEINST = 1 diff --git a/ground/src/plugins/uavobjects/actuatorsettings.cpp b/ground/src/plugins/uavobjects/actuatorsettings.cpp index c2062cc15..4819024a9 100644 --- a/ground/src/plugins/uavobjects/actuatorsettings.cpp +++ b/ground/src/plugins/uavobjects/actuatorsettings.cpp @@ -259,6 +259,30 @@ ActuatorSettings::ActuatorSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTI ChannelMinElemNames.append("6"); ChannelMinElemNames.append("7"); fields.append( new UAVObjectField(QString("ChannelMin"), QString("us"), UAVObjectField::INT16, ChannelMinElemNames, QStringList()) ); + QStringList ChannelTypeElemNames; + ChannelTypeElemNames.append("0"); + ChannelTypeElemNames.append("1"); + ChannelTypeElemNames.append("2"); + ChannelTypeElemNames.append("3"); + ChannelTypeElemNames.append("4"); + ChannelTypeElemNames.append("5"); + ChannelTypeElemNames.append("6"); + ChannelTypeElemNames.append("7"); + QStringList ChannelTypeEnumOptions; + ChannelTypeEnumOptions.append("PWM"); + ChannelTypeEnumOptions.append("MK"); + ChannelTypeEnumOptions.append("ASTEC4"); + fields.append( new UAVObjectField(QString("ChannelType"), QString(""), UAVObjectField::ENUM, ChannelTypeElemNames, ChannelTypeEnumOptions) ); + QStringList ChannelAddrElemNames; + ChannelAddrElemNames.append("0"); + ChannelAddrElemNames.append("1"); + ChannelAddrElemNames.append("2"); + ChannelAddrElemNames.append("3"); + ChannelAddrElemNames.append("4"); + ChannelAddrElemNames.append("5"); + ChannelAddrElemNames.append("6"); + ChannelAddrElemNames.append("7"); + fields.append( new UAVObjectField(QString("ChannelAddr"), QString(""), UAVObjectField::UINT8, ChannelAddrElemNames, QStringList()) ); // Initialize object initializeFields(fields, (quint8*)&data, NUMBYTES); @@ -334,6 +358,22 @@ void ActuatorSettings::setDefaultFieldValues() data.ChannelMin[5] = 1000; data.ChannelMin[6] = 1000; data.ChannelMin[7] = 1000; + data.ChannelType[0] = 0; + data.ChannelType[1] = 0; + data.ChannelType[2] = 0; + data.ChannelType[3] = 0; + data.ChannelType[4] = 0; + data.ChannelType[5] = 0; + data.ChannelType[6] = 0; + data.ChannelType[7] = 0; + data.ChannelAddr[0] = 0; + data.ChannelAddr[1] = 1; + data.ChannelAddr[2] = 2; + data.ChannelAddr[3] = 3; + data.ChannelAddr[4] = 4; + data.ChannelAddr[5] = 5; + data.ChannelAddr[6] = 6; + data.ChannelAddr[7] = 7; } diff --git a/ground/src/plugins/uavobjects/actuatorsettings.h b/ground/src/plugins/uavobjects/actuatorsettings.h index caeb32cda..544ab7564 100644 --- a/ground/src/plugins/uavobjects/actuatorsettings.h +++ b/ground/src/plugins/uavobjects/actuatorsettings.h @@ -61,6 +61,8 @@ public: qint16 ChannelMax[8]; qint16 ChannelNeutral[8]; qint16 ChannelMin[8]; + quint8 ChannelType[8]; + quint8 ChannelAddr[8]; } __attribute__((packed)) DataFields; @@ -119,10 +121,18 @@ public: // Field ChannelMin information /* Number of elements for field ChannelMin */ static const quint32 CHANNELMIN_NUMELEM = 8; + // Field ChannelType information + /* Enumeration options for field ChannelType */ + typedef enum { CHANNELTYPE_PWM=0, CHANNELTYPE_MK=1, CHANNELTYPE_ASTEC4=2 } ChannelTypeOptions; + /* Number of elements for field ChannelType */ + static const quint32 CHANNELTYPE_NUMELEM = 8; + // Field ChannelAddr information + /* Number of elements for field ChannelAddr */ + static const quint32 CHANNELADDR_NUMELEM = 8; // Constants - static const quint32 OBJID = 3054509114U; + static const quint32 OBJID = 844831578U; static const QString NAME; static const QString DESCRIPTION; static const bool ISSINGLEINST = 1; diff --git a/ground/src/plugins/uavobjects/actuatorsettings.py b/ground/src/plugins/uavobjects/actuatorsettings.py index 0122913ea..bfc9ff825 100644 --- a/ground/src/plugins/uavobjects/actuatorsettings.py +++ b/ground/src/plugins/uavobjects/actuatorsettings.py @@ -365,12 +365,49 @@ _fields = [ \ { } ), + uavobject.UAVObjectField( + 'ChannelType', + 'b', + 8, + [ + '0', + '1', + '2', + '3', + '4', + '5', + '6', + '7', + ], + { + '0' : 'PWM', + '1' : 'MK', + '2' : 'ASTEC4', + } + ), + uavobject.UAVObjectField( + 'ChannelAddr', + 'B', + 8, + [ + '0', + '1', + '2', + '3', + '4', + '5', + '6', + '7', + ], + { + } + ), ] class ActuatorSettings(uavobject.UAVObject): ## Object constants - OBJID = 3054509114 + OBJID = 844831578 NAME = "ActuatorSettings" METANAME = "ActuatorSettingsMeta" ISSINGLEINST = 1 diff --git a/ground/src/shared/uavobjectdefinition/actuatorcommand.xml b/ground/src/shared/uavobjectdefinition/actuatorcommand.xml index 56ff36370..5f9640fee 100644 --- a/ground/src/shared/uavobjectdefinition/actuatorcommand.xml +++ b/ground/src/shared/uavobjectdefinition/actuatorcommand.xml @@ -2,6 +2,9 @@ Contains the pulse duration sent to each of the channels. Set by @ref ActuatorModule + + + diff --git a/ground/src/shared/uavobjectdefinition/actuatorsettings.xml b/ground/src/shared/uavobjectdefinition/actuatorsettings.xml index eb444b560..bdafbe834 100644 --- a/ground/src/shared/uavobjectdefinition/actuatorsettings.xml +++ b/ground/src/shared/uavobjectdefinition/actuatorsettings.xml @@ -19,6 +19,8 @@ + +