diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index cb41469c8..c311f05d3 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -280,11 +280,11 @@ void INSSetBaroVar(float baro_var) void INSSetMagNorth(float B[3]) { - float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); + float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); - ekf.Be[0] = B[0] / mag; - ekf.Be[1] = B[1] / mag; - ekf.Be[2] = B[2] / mag; + ekf.Be[0] = B[0] * invmag; + ekf.Be[1] = B[1] * invmag; + ekf.Be[2] = B[2] * invmag; } void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) @@ -305,7 +305,7 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) // EKF prediction step LinearizeFG(ekf.X, U, ekf.F, ekf.G); RungeKutta(ekf.X, U, dT); - invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); ekf.X[6] *= invqmag; ekf.X[7] *= invqmag; ekf.X[8] *= invqmag; @@ -390,7 +390,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], if (SensorsUsed & MAG_SENSORS) { - float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); + float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); Z[6] = mag_data[0] * invBmag; Z[7] = mag_data[1] * invBmag; Z[8] = mag_data[2] * invBmag; @@ -404,7 +404,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], MeasurementEq(ekf.X, ekf.Be, Y); SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); - float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + float invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); ekf.X[6] *= invqmag; ekf.X[7] *= invqmag; ekf.X[8] *= invqmag; diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index 68026f682..859375258 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -119,28 +119,12 @@ static inline float y_on_curve(float x, const pointf points[], int num_points) // Find the y value on the selected line. return y_on_line(x, &points[end_point - 1], &points[end_point]); } -// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c" -// http://en.wikipedia.org/wiki/Fast_inverse_square_root -static inline float fast_invsqrtf(float number) +static inline float invsqrtf(float number) { - float x2, y; - const float threehalfs = 1.5F; - - union { - float f; - uint32_t u; - } i; - - x2 = number * 0.5F; - y = number; - - i.f = y; // evil floating point bit level hacking - i.u = 0x5f3759df - (i.u >> 1); // what the fxck? - y = i.f; - y = y * (threehalfs - (x2 * y * y)); // 1st iteration -// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed + float y; + y = 1.0f / sqrtf(number); return y; } diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index 03e67ed47..e0f837e24 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -88,6 +88,7 @@ SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_hott.c SRC += $(PIOSCOMMON)/pios_srxl.c +SRC += $(PIOSCOMMON)/pios_exbus.c SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_sensors.c diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index c9d5fd5f8..f2d024d24 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -628,7 +628,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); // Account for accel magnitude - float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); + float inv_accel_mag = invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); if (inv_accel_mag > 1e3f) { return; } @@ -637,7 +637,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel float inv_grot_mag; if (accel_filter_enabled) { - inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); + inv_grot_mag = invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); } else { inv_grot_mag = 1.0f; } @@ -685,8 +685,8 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel } } - // Renomalize - float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + // Renormalize + float inv_qmag = invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 7cee789ee..90ea959a6 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -99,7 +99,7 @@ static uint8_t isAssistedFlightMode(uint8_t position); static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, uint8_t deadband, float dT); #endif -#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 18 // Sbus max channel +#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 18 // Sbus max channel #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 15 struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; @@ -655,14 +655,14 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_EXBUS; + break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTMAINPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTFLEXIPORT; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT: + group = RECEIVERACTIVITY_ACTIVEGROUP_HOTT; break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL: group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL; diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 9d5ad1374..b1c44bd9a 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -438,16 +438,16 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float attitude[3] = -attitude[3]; } - // Renomalize - float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]); - attitude[0] = attitude[0] / qmag; - attitude[1] = attitude[1] / qmag; - attitude[2] = attitude[2] / qmag; - attitude[3] = attitude[3] / qmag; + // Renormalize + float inv_qmag = invsqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]); + attitude[0] = attitude[0] * inv_qmag; + attitude[1] = attitude[1] * inv_qmag; + attitude[2] = attitude[2] * inv_qmag; + attitude[3] = attitude[3] * inv_qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { + if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) { this->first_run = 1; return FILTERRESULT_WARNING; } diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index dad266505..7a3d4e998 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -114,8 +114,8 @@ typedef struct { #endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Transmit/receive task handles - xTaskHandle txTaskHandle; - xTaskHandle rxTaskHandle; + xTaskHandle txTaskHandle; + xTaskHandle rxTaskHandle; // Telemetry stream UAVTalkConnection uavTalkCon; } channelContext; @@ -170,7 +170,6 @@ static void gcsTelemetryStatsUpdated(); */ int32_t TelemetryStart(void) { - #ifdef HAS_RADIO // Only start the local telemetry tasks if needed if (localPort()) { @@ -623,7 +622,6 @@ static void telemetryTxTask(void *parameters) processObjEvent(channel, &ev); } #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - } } diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c new file mode 100644 index 000000000..d2b25f677 --- /dev/null +++ b/flight/pios/common/pios_exbus.c @@ -0,0 +1,439 @@ +/** + ****************************************************************************** + * @file pios_exbus.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EXBUS Jeti EX Bus receiver functions + * @{ + * @brief Jeti EX Bus receiver functions + *****************************************************************************/ +/* + * 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_exbus_priv.h" + +#if defined(PIOS_INCLUDE_EXBUS) + +#if !defined(PIOS_INCLUDE_RTC) +#error PIOS_INCLUDE_RTC must be used to use EXBUS +#endif + +/** + * Based on Jeti EX Bus protocol version 1.21 + * Available at http://www.jetimodel.com/en/Telemetry-Protocol/ + */ + +/* EXBUS frame size and contents definitions */ +#define EXBUS_HEADER_LENGTH 4 +#define EXBUS_CRC_LENGTH 2 +#define EXBUS_MAX_CHANNELS 16 +#define EXBUS_OVERHEAD_LENGTH (EXBUS_HEADER_LENGTH + EXBUS_CRC_LENGTH) +#define EXBUS_MAX_FRAME_LENGTH (EXBUS_MAX_CHANNELS*2+10 + EXBUS_OVERHEAD_LENGTH) + +#define EXBUS_SYNC_CHANNEL 0x3E +#define EXBUS_SYNC_TELEMETRY 0x3D +#define EXBUS_BYTE_REQ 0x01 +#define EXBUS_BYTE_NOREQ 0x03 + +#define EXBUS_DATA_CHANNEL 0x31 +#define EXBUS_DATA_TELEMETRY 0x3A +#define EXBUS_DATA_JETIBOX 0x3B + +#define EXBUS_LOW_BAUD_RATE 125000 +#define EXBUS_HIGH_BAUD_RATE 250000 +#define EXBUS_BAUD_RATE_LIMIT 64 + +/* Forward Declarations */ +static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel); +static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); +static void PIOS_EXBUS_Supervisor(uint32_t exbus_id); +static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data); + +/* Local Variables */ +const struct pios_rcvr_driver pios_exbus_rcvr_driver = { + .read = PIOS_EXBUS_Get, +}; + +enum pios_exbus_dev_magic { + PIOS_EXBUS_DEV_MAGIC = 0x485355FF, +}; + +enum pios_exbus_frame_state { + EXBUS_STATE_SYNC, + EXBUS_STATE_REQ, + EXBUS_STATE_LEN, + EXBUS_STATE_PACKET_ID, + EXBUS_STATE_DATA_ID, + EXBUS_STATE_SUBLEN, + EXBUS_STATE_DATA +}; + +struct pios_exbus_state { + uint16_t channel_data[PIOS_EXBUS_NUM_INPUTS]; + uint8_t received_data[EXBUS_MAX_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t failsafe_count; + uint8_t byte_count; + uint8_t frame_length; + uint16_t crc; + bool high_baud_rate; + bool frame_found; +}; + +struct pios_exbus_dev { + enum pios_exbus_dev_magic magic; + uint32_t com_port_id; + const struct pios_com_driver *driver; + struct pios_exbus_state state; +}; + +/* Allocate EXBUS device descriptor */ +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_exbus_dev *PIOS_EXBUS_Alloc(void) +{ + struct pios_exbus_dev *exbus_dev; + + exbus_dev = (struct pios_exbus_dev *)pios_malloc(sizeof(*exbus_dev)); + if (!exbus_dev) + return NULL; + + exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; + return exbus_dev; +} +#else +static struct pios_exbus_dev pios_exbus_devs[PIOS_EXBUS_MAX_DEVS]; +static uint8_t pios_exbus_num_devs; +static struct pios_exbus_dev *PIOS_EXBUS_Alloc(void) +{ + struct pios_exbus_dev *exbus_dev; + + if (pios_exbus_num_devs >= PIOS_EXBUS_MAX_DEVS) + return NULL; + + exbus_dev = &pios_exbus_devs[pios_exbus_num_devs++]; + exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; + + return exbus_dev; +} +#endif + +/* Validate EXBUS device descriptor */ +static bool PIOS_EXBUS_Validate(struct pios_exbus_dev *exbus_dev) +{ + return (exbus_dev->magic == PIOS_EXBUS_DEV_MAGIC); +} + +/* Reset channels in case of lost signal or explicit failsafe receiver flag */ +static void PIOS_EXBUS_ResetChannels(struct pios_exbus_dev *exbus_dev) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + for (int i = 0; i < PIOS_EXBUS_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } +} + +/* Reset EXBUS receiver state */ +static void PIOS_EXBUS_ResetState(struct pios_exbus_dev *exbus_dev) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + state->receive_timer = 0; + state->failsafe_timer = 0; + state->failsafe_count = 0; + state->high_baud_rate = false; + state->frame_found = false; + PIOS_EXBUS_ResetChannels(exbus_dev); +} + +/** + * Check and unroll complete frame data. + * \output 0 frame data accepted + * \output -1 frame error found + */ +static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + + if(state->crc != 0) { + /* crc failed */ + DEBUG_PRINTF(2, "Jeti CRC error!%d\r\n"); + return -1; + } + + enum pios_exbus_frame_state exbus_state = EXBUS_STATE_SYNC; + uint8_t *byte = state->received_data; + uint8_t sub_length; + uint8_t n_channels = 0; + uint8_t channel = 0; + + while((byte - state->received_data) < state->frame_length) { + switch(exbus_state) { + case EXBUS_STATE_SYNC: + /* sync byte */ + if(*byte == EXBUS_SYNC_CHANNEL) { + exbus_state = EXBUS_STATE_REQ; + } + else { + return -1; + } + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_REQ: + /* + * tells us whether the rx is requesting a reply or not, + * currently nothing is actually done with this information... + */ + if(*byte == EXBUS_BYTE_REQ) { + exbus_state = EXBUS_STATE_LEN; + } + else if(*byte == EXBUS_BYTE_NOREQ) { + exbus_state = EXBUS_STATE_LEN; + } + else { + return -1; + } + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_LEN: + /* total frame length */ + exbus_state = EXBUS_STATE_PACKET_ID; + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_PACKET_ID: + /* packet id */ + exbus_state = EXBUS_STATE_DATA_ID; + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_DATA_ID: + /* checks the type of data, ignore non-rc data */ + if(*byte == EXBUS_DATA_CHANNEL) { + exbus_state = EXBUS_STATE_SUBLEN; + } + else { + return -1; + } + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_SUBLEN: + sub_length = *byte; + n_channels = sub_length / 2; + exbus_state = EXBUS_STATE_DATA; + byte += sizeof(uint8_t); + break; + case EXBUS_STATE_DATA: + if(channel < n_channels) { + /* 1 lsb = 1/8 us */ + state->channel_data[channel++] = (byte[1] << 8 | byte[0]) / 8; + } + byte += sizeof(uint16_t); + break; + } + } + return 0; +} + +/* Update decoder state processing input byte from the stream */ +static void PIOS_EXBUS_UpdateState(struct pios_exbus_dev *exbus_dev, uint8_t byte) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + + if(!state->frame_found) { + if(byte == EXBUS_SYNC_CHANNEL) { + state->frame_found = true; + state->byte_count = 0; + state->frame_length = EXBUS_MAX_FRAME_LENGTH; + state->crc = PIOS_EXBUS_CRC_Update(0, byte); + state->received_data[state->byte_count++] = byte; + } + } + else if(state->byte_count < EXBUS_MAX_FRAME_LENGTH) { + state->crc = PIOS_EXBUS_CRC_Update(state->crc, byte); + state->received_data[state->byte_count++] = byte; + + if(state->byte_count == 3) { + state->frame_length = byte; + } + if(state->byte_count == state->frame_length) { + if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) { + /* data looking good */ + state->failsafe_timer = 0; + state->failsafe_count = 0; + } + /* get ready for the next frame */ + state->frame_found = false; + } + } + else { + state->frame_found = false; + } +} + +/* Initialise EX Bus receiver interface */ +int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, + const struct pios_com_driver *driver, + uint32_t lower_id) +{ + PIOS_DEBUG_Assert(exbus_id); + PIOS_DEBUG_Assert(driver); + + struct pios_exbus_dev *exbus_dev; + + exbus_dev = (struct pios_exbus_dev *)PIOS_EXBUS_Alloc(); + if (!exbus_dev) + return -1; + + PIOS_EXBUS_ResetState(exbus_dev); + + *exbus_id = (uint32_t)exbus_dev; + + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_EXBUS_RxInCallback, *exbus_id); + + if (!PIOS_RTC_RegisterTickCallback(PIOS_EXBUS_Supervisor, *exbus_id)) { + PIOS_DEBUG_Assert(0); + } + + exbus_dev->driver = driver; + exbus_dev->com_port_id = lower_id; + + return 0; +} + +/* Comm byte received callback */ +static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) +{ + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)context; + + bool valid = PIOS_EXBUS_Validate(exbus_dev); + PIOS_Assert(valid); + + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_EXBUS_UpdateState(exbus_dev, buf[i]); + exbus_dev->state.receive_timer = 0; + } + + /* Always signal that we can accept more data */ + if (headroom) + *headroom = EXBUS_MAX_FRAME_LENGTH; + + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; +} + +/** + * Get the value of an input channel + * \param[in] channel Number of the channel desired (zero based) + * \output PIOS_RCVR_INVALID channel not available + * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver + * \output >=0 channel value + */ +static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)rcvr_id; + + if (!PIOS_EXBUS_Validate(exbus_dev)) + return PIOS_RCVR_INVALID; + + /* return error if channel is not available */ + if (channel >= PIOS_EXBUS_NUM_INPUTS) + return PIOS_RCVR_INVALID; + + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return exbus_dev->state.channel_data[channel]; +} + +static void PIOS_EXBUS_Change_BaudRate(struct pios_exbus_dev *device) { + struct pios_exbus_state *state = &(device->state); + if (++state->failsafe_count >= EXBUS_BAUD_RATE_LIMIT) { + state->high_baud_rate = !state->high_baud_rate; + (device->driver->set_baud) (device->com_port_id, + state->high_baud_rate ? EXBUS_HIGH_BAUD_RATE : EXBUS_LOW_BAUD_RATE); + state->failsafe_count = 0; + } +} + +/** + * Input data supervisor is called periodically and provides + * two functions: frame syncing and failsafe triggering. + * + * EX.Bus frames come at 20ms or 10ms rate at 125 or 250 kbaud. + * RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives + * 8ms pause between frames which is good for both EX.Bus frame rates. + * + * Data receive function must clear the receive_timer to confirm new + * data reception. If no new data received in 100ms, we must call the + * failsafe function which clears all channels. + */ +static void PIOS_EXBUS_Supervisor(uint32_t exbus_id) +{ + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)exbus_id; + + bool valid = PIOS_EXBUS_Validate(exbus_dev); + PIOS_Assert(valid); + + struct pios_exbus_state *state = &(exbus_dev->state); + + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = false; + state->byte_count = 0; + state->receive_timer = 0; + state->frame_length = EXBUS_MAX_FRAME_LENGTH; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_EXBUS_ResetChannels(exbus_dev); + state->failsafe_timer = 0; + PIOS_EXBUS_Change_BaudRate(exbus_dev); + } +} + +static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data) +{ + data ^= (uint8_t)crc & (uint8_t)0xFF; + data ^= data << 4; + crc = ((((uint16_t)data << 8) | ((crc & 0xFF00) >> 8)) + ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3)); + + return crc; +} + +#endif /* PIOS_INCLUDE_EXBUS */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_exbus.h b/flight/pios/inc/pios_exbus.h new file mode 100644 index 000000000..58d4a47d5 --- /dev/null +++ b/flight/pios/inc/pios_exbus.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SBus Futaba S.Bus receiver functions + * @{ + * + * @file pios_sbus.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @brief Futaba S.Bus functions header. + * @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_EXBUS_H +#define PIOS_EXBUS_H + +/* Global Types */ + +/* Public Functions */ + +#endif /* PIOS_EXBUS_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_exbus_priv.h b/flight/pios/inc/pios_exbus_priv.h new file mode 100644 index 000000000..e922fef57 --- /dev/null +++ b/flight/pios/inc/pios_exbus_priv.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @file pios_exbus_priv.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EXBUS Jeti EX Bus receiver functions + * @{ + * @brief Jeti EX Bus receiver functions + *****************************************************************************/ +/* + * 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_EXBUS_PRIV_H +#define PIOS_EXBUS_PRIV_H + +#include +#include + + +/* EXBUS receiver instance configuration */ +extern const struct pios_rcvr_driver pios_exbus_rcvr_driver; + +extern int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, + const struct pios_com_driver *driver, + uint32_t lower_id); + +#endif /* PIOS_EXBUS_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index 5016d6750..c3b3683e1 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -341,6 +341,11 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, } } #endif /* USE_FREERTOS */ + } else { + for (uint32_t i = 0; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } } ppm_dev->Fresh = TRUE; diff --git a/flight/pios/stm32f4xx/pios_bl_helper.c b/flight/pios/stm32f4xx/pios_bl_helper.c index acf3d11b6..8b2150450 100644 --- a/flight/pios/stm32f4xx/pios_bl_helper.c +++ b/flight/pios/stm32f4xx/pios_bl_helper.c @@ -238,13 +238,13 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) PIOS_Assert(0); } for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) { - //if erasing area contain whole bank2 area, using bank erase - //bank2: sector 12 to sector 23 + // if erasing area contain whole bank2 area, using bank erase + // bank2: sector 12 to sector 23 if (sector_start == flash_sectors[12].start && - endAddress >= (flash_sectors[23].start + flash_sectors[23].size)){ + endAddress >= (flash_sectors[23].start + flash_sectors[23].size)) { if (FLASH_EraseAllBank2Sectors(VoltageRange_3) == FLASH_COMPLETE) { fail = false; - //using bank2 total size substitute sector_size + // using bank2 total size substitute sector_size sector_size = flash_sectors[23].start - flash_sectors[12].start + flash_sectors[23].size; break; } else { diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index 9d1deeecd..809ceae9b 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -330,6 +330,11 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32 } } #endif /* USE_FREERTOS */ + } else { + for (uint32_t i = 0; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } } ppm_dev->Fresh = true; diff --git a/flight/pios/stm32f4xx/pios_sys.c b/flight/pios/stm32f4xx/pios_sys.c index e618b0e7e..ab6759fe9 100644 --- a/flight/pios/stm32f4xx/pios_sys.c +++ b/flight/pios/stm32f4xx/pios_sys.c @@ -74,7 +74,7 @@ void PIOS_SYS_Init(void) RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_GPIOE | -#if defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F429_439xx) +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) RCC_AHB1Periph_GPIOF | RCC_AHB1Periph_GPIOG | RCC_AHB1Periph_GPIOH | @@ -85,7 +85,7 @@ void PIOS_SYS_Init(void) RCC_AHB1Periph_SRAM1 | RCC_AHB1Periph_SRAM2 | RCC_AHB1Periph_BKPSRAM | -#if defined (STM32F427_437xx) || defined (STM32F429_439xx) +#if defined(STM32F427_437xx) || defined(STM32F429_439xx) RCC_AHB1Periph_SRAM3 | #endif RCC_AHB1Periph_DMA1 | @@ -174,7 +174,7 @@ void PIOS_SYS_Init(void) GPIO_Init(GPIOD, &GPIO_InitStructure); GPIO_Init(GPIOE, &GPIO_InitStructure); -#if defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F429_439xx) +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) GPIO_Init(GPIOF, &GPIO_InitStructure); GPIO_Init(GPIOG, &GPIO_InitStructure); GPIO_Init(GPIOH, &GPIO_InitStructure); diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index f4cde50f4..b693a7e9b 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -1027,42 +1027,6 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { */ #include -static const struct pios_usart_cfg pios_usart_hott_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { .regs = USART3, .init = { @@ -1098,9 +1062,51 @@ static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { }, }, }; - #endif /* PIOS_INCLUDE_HOTT */ +#if defined(PIOS_INCLUDE_EXBUS) +/* + * EXBUS USART + */ +#include + +static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 125000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; +#endif /* PIOS_INCLUDE_EXBUS */ + #if defined(PIOS_INCLUDE_SRXL) /* * SRXL USART diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 54b1d3039..8bf8f3634 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -101,6 +101,7 @@ #define PIOS_INCLUDE_PPM_FLEXI #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_SRXL #define PIOS_INCLUDE_HOTT /* #define PIOS_INCLUDE_GCSRCVR */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index ef1d2b4ec..e10bed8c4 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -518,29 +518,6 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; - case HWSETTINGS_CC_MAINPORT_HOTTSUMD: - case HWSETTINGS_CC_MAINPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) @@ -717,10 +694,32 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; } #endif /* PIOS_INCLUDE_HOTT */ break; + + case HWSETTINGS_CC_FLEXIPORT_EXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; case HWSETTINGS_CC_FLEXIPORT_SRXL: #if defined(PIOS_INCLUDE_SRXL) diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 6fad48ef9..f6c272f8d 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -251,9 +251,15 @@ extern uint32_t pios_com_hkosd_id; //------------------------- // Receiver HSUM input //------------------------- -#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_MAX_DEVS 1 #define PIOS_HOTT_NUM_INPUTS 32 +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index b0e1904dc..77d53d8db 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1,8 +1,9 @@ /** ****************************************************************************** * @file board_hw_defs.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core @@ -1054,6 +1055,7 @@ static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { }; #endif /* PIOS_INCLUDE_SRXL */ + #if defined(PIOS_INCLUDE_HOTT) /* * HOTT USART @@ -1101,11 +1103,19 @@ static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_hott_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, +#endif /* PIOS_INCLUDE_HOTT */ + +#if defined(PIOS_INCLUDE_EXBUS) +/* + * EXBUS USART + */ +#include + +static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { + .regs = USART3, + .remap = GPIO_AF_USART3, .init = { - .USART_BaudRate = 115200, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, @@ -1114,16 +1124,16 @@ static const struct pios_usart_cfg pios_usart_hott_main_cfg = { }, .irq = { .init = { - .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }, }, .rx = { - .gpio = GPIOA, + .gpio = GPIOB, .init = { - .GPIO_Pin = GPIO_Pin_10, + .GPIO_Pin = GPIO_Pin_11, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_AF, .GPIO_OType = GPIO_OType_PP, @@ -1131,18 +1141,19 @@ static const struct pios_usart_cfg pios_usart_hott_main_cfg = { }, }, .tx = { - .gpio = GPIOA, + .gpio = GPIOB, .init = { - .GPIO_Pin = GPIO_Pin_9, + .GPIO_Pin = GPIO_Pin_10, .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Mode = GPIO_Mode_OUT, .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, }, }; -#endif /* PIOS_INCLUDE_HOTT */ +#endif /* PIOS_INCLUDE_EXBUS */ + /* * HK OSD */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index bced97f12..e41efc7b1 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -5,7 +5,8 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 @@ -105,6 +106,7 @@ #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL #define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_GCSRCVR #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 7e8f718cb..a0cdd6d04 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -1,8 +1,9 @@ /** ****************************************************************************** * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core @@ -547,6 +548,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_FLEXIPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; + case HWSETTINGS_RM_FLEXIPORT_SRXL: #if defined(PIOS_INCLUDE_SRXL) { @@ -566,8 +568,9 @@ void PIOS_Board_Init(void) } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; } -#endif +#endif /* PIOS_INCLUDE_SRXL */ break; + case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD: case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HOTT) @@ -587,11 +590,33 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; } #endif /* PIOS_INCLUDE_HOTT */ break; + case HWSETTINGS_RM_FLEXIPORT_EXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; + } /* hwsettings_rm_flexiport */ /* Moved this here to allow binding on flexiport */ @@ -775,30 +800,6 @@ void PIOS_Board_Init(void) PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); break; - case HWSETTINGS_RM_MAINPORT_HOTTSUMD: - case HWSETTINGS_RM_MAINPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_mainport == HWSETTINGS_RM_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index feb778a78..2620f068b 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -5,7 +5,8 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * @@ -260,9 +261,15 @@ extern uint32_t pios_packet_handler; //------------------------- // Receiver HSUM input //------------------------- -#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_MAX_DEVS 1 #define PIOS_HOTT_NUM_INPUTS 32 +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index b0ca380e2..7c651de48 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -1,7 +1,8 @@ /** ****************************************************************************** * @file board_hw_defs.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core @@ -540,46 +541,6 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { * HOTT USART */ #include -static const struct pios_usart_cfg pios_usart_hott_main_cfg = { - .regs = MAIN_USART_REGS, - .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = MAIN_USART_TX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { .regs = FLEXI_USART_REGS, @@ -672,6 +633,56 @@ static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { }; #endif /* PIOS_INCLUDE_SRXL */ + +#if defined(PIOS_INCLUDE_EXBUS) +/* + * EXBUS USART + */ +#include + +static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { + .regs = FLEXI_USART_REGS, + .remap = FLEXI_USART_REMAP, + .init = { + .USART_BaudRate = 125000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = FLEXI_USART_IRQ, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = FLEXI_USART_RX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_RX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = FLEXI_USART_TX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_TX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + + +#endif /* PIOS_INCLUDE_EXBUS */ /* * HK OSD */ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index d9cff34a1..7cfe391db 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -5,7 +5,8 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 @@ -109,6 +110,7 @@ #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL #define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_GCSRCVR // #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 459895f09..f4786a09f 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -1,8 +1,9 @@ /** ****************************************************************************** * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core @@ -605,30 +606,6 @@ void PIOS_Board_Init(void) &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); } break; - case HWSETTINGS_RM_MAINPORT_HOTTSUMD: - case HWSETTINGS_RM_MAINPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_mainport == HWSETTINGS_RM_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { @@ -738,6 +715,7 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD: case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HOTT) @@ -757,10 +735,33 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; } #endif /* PIOS_INCLUDE_HOTT */ break; + + case HWSETTINGS_RM_FLEXIPORT_EXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; + } /* hwsettings_rm_flexiport */ diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 0805c1a5f..0c944fb82 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -5,7 +5,8 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * @@ -260,9 +261,15 @@ extern uint32_t pios_packet_handler; //------------------------- // Receiver HSUM input //------------------------- -#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_MAX_DEVS 1 #define PIOS_HOTT_NUM_INPUTS 32 +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/ground/gcs/src/libs/osgearth/copydata.pro b/ground/gcs/src/libs/osgearth/copydata.pro index bd93a4fe1..e28e3d7b9 100644 --- a/ground/gcs/src/libs/osgearth/copydata.pro +++ b/ground/gcs/src/libs/osgearth/copydata.pro @@ -36,7 +36,7 @@ equals(copyosg, 1) { win32 { # set debug suffix if needed - CONFIG(debug, debug|release):DS = "d" + #CONFIG(debug, debug|release):DS = "d" # copy osg libraries OSG_LIBS = \ diff --git a/ground/gcs/src/libs/osgearth/osgearth.pro b/ground/gcs/src/libs/osgearth/osgearth.pro index 4ee5a7dc2..a7185ada0 100644 --- a/ground/gcs/src/libs/osgearth/osgearth.pro +++ b/ground/gcs/src/libs/osgearth/osgearth.pro @@ -91,18 +91,18 @@ macx { win32 { LIBS += -L$$OSG_SDK_DIR/lib - CONFIG(release, debug|release) { + #CONFIG(release, debug|release) { LIBS += -lOpenThreads LIBS += -losg -losgUtil -losgDB -losgGA -losgViewer -losgText LIBS += -losgEarth -losgEarthUtil -losgEarthFeatures -losgEarthSymbology -losgEarthAnnotation LIBS += -losgQt -losgEarthQt - } - CONFIG(debug, debug|release) { - LIBS += -lOpenThreadsd - LIBS += -losgd -losgUtild -losgDBd -losgGAd -losgViewerd -losgTextd - LIBS += -losgEarthd -losgEarthUtild -losgEarthFeaturesd -losgEarthSymbologyd -losgEarthAnnotationd - LIBS += -losgQtd -losgEarthQtd - } + #} + #CONFIG(debug, debug|release) { + # LIBS += -lOpenThreadsd + # LIBS += -losgd -losgUtild -losgDBd -losgGAd -losgViewerd -losgTextd + # LIBS += -losgEarthd -losgEarthUtild -losgEarthFeaturesd -losgEarthSymbologyd -losgEarthAnnotationd + # LIBS += -losgQtd -losgEarthQtd + #} } include(copydata.pro) diff --git a/ground/gcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/gcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index e90f838e9..751f7c36d 100644 --- a/ground/gcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/gcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -230,9 +230,9 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) setComboCurrentIndex(m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Hexacopter Y6")); - m_aircraft->mrRollMixLevel->setValue(100); - m_aircraft->mrPitchMixLevel->setValue(50); - setYawMixLevel(66); + m_aircraft->mrRollMixLevel->setValue(86); + m_aircraft->mrPitchMixLevel->setValue(100); + setYawMixLevel(100); } else if (frameType == "Octo" || frameType == "Octocopter") { setComboCurrentIndex(m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter")); diff --git a/ground/gcs/src/plugins/config/configstabilizationwidget.cpp b/ground/gcs/src/plugins/config/configstabilizationwidget.cpp index 519bc842b..5c5f2ecdd 100644 --- a/ground/gcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/gcs/src/plugins/config/configstabilizationwidget.cpp @@ -245,8 +245,22 @@ void ConfigStabilizationWidget::refreshWidgetsValues(UAVObject *o) updateThrottleCurveFromObject(); - ui->basicResponsivenessCheckBox->setChecked(ui->rateRollKp_3->value() == ui->ratePitchKp_4->value() && - ui->rateRollKi_3->value() == ui->ratePitchKi_4->value()); + // Check and update basic/advanced checkboxes only if something connected + // Jump to advanced tab if something not "basic": Rate value out of slider limits or different Pitch/Roll values + if (ui->lowThrottleZeroIntegral_8->isEnabled()) { + if ((ui->attitudeRollResponse->value() == ui->attitudePitchResponse->value()) && + (ui->rateRollResponse->value() == ui->ratePitchResponse->value()) && + (ui->rateRollResponse->value() <= ui->RateResponsivenessSlider->maximum()) && + (ui->ratePitchResponse->value() <= ui->RateResponsivenessSlider->maximum())) { + ui->basicResponsivenessCheckBox->setChecked(true); + ui->advancedResponsivenessCheckBox->setChecked(false); + ui->tabWidget->setCurrentIndex(0); + } else { + ui->basicResponsivenessCheckBox->setChecked(false); + ui->advancedResponsivenessCheckBox->setChecked(true); + ui->tabWidget->setCurrentIndex(1); + } + } } void ConfigStabilizationWidget::updateObjectsFromWidgets() @@ -611,9 +625,11 @@ void ConfigStabilizationWidget::processLinkedWidgets(QWidget *widget) if (ui->basicResponsivenessCheckBox->isChecked()) { if (widget == ui->AttitudeResponsivenessSlider) { - ui->ratePitchKp_4->setValue(ui->AttitudeResponsivenessSlider->value()); + ui->attitudePitchResponse->setValue(ui->AttitudeResponsivenessSlider->value()); + ui->attitudeRollResponse->setValue(ui->AttitudeResponsivenessSlider->value()); } else if (widget == ui->RateResponsivenessSlider) { - ui->ratePitchKi_4->setValue(ui->RateResponsivenessSlider->value()); + ui->ratePitchResponse->setValue(ui->RateResponsivenessSlider->value()); + ui->rateRollResponse->setValue(ui->RateResponsivenessSlider->value()); } } if (ui->checkBoxLinkAcroFactors->isChecked()) { diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index 135e1b64c..5a4f20622 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -151,14 +151,14 @@ void InputChannelForm::groupUpdated() case ManualControlSettings::CHANNELGROUPS_DSMFLEXIPORT: count = 12; break; + case ManualControlSettings::CHANNELGROUPS_SRXL: + case ManualControlSettings::CHANNELGROUPS_EXBUS: + count = 16; + break; case ManualControlSettings::CHANNELGROUPS_SBUS: count = 18; break; - case ManualControlSettings::CHANNELGROUPS_SRXL: - count = 16; - break; - case ManualControlSettings::CHANNELGROUPS_HOTTMAINPORT: - case ManualControlSettings::CHANNELGROUPS_HOTTFLEXIPORT: + case ManualControlSettings::CHANNELGROUPS_HOTT: count = 32; break; case ManualControlSettings::CHANNELGROUPS_GCS: diff --git a/ground/gcs/src/plugins/config/stabilization.ui b/ground/gcs/src/plugins/config/stabilization.ui index 811b6735d..2dad4132f 100644 --- a/ground/gcs/src/plugins/config/stabilization.ui +++ b/ground/gcs/src/plugins/config/stabilization.ui @@ -195,29 +195,28 @@ margin-top: -1px; Acro+ - - - - - - 0 - 0 - - - - Qt::StrongFocus - - - <html><head/><body><p>Link roll &amp; pitch sliders to move together</p></body></html> - - - - - - Link Roll and Pitch - - - + + + + + 0 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Link roll &amp; pitch sliders to move together</p></body></html> + + + + + + Link Roll and Pitch + + + @@ -1002,7 +1001,7 @@ margin-top: -1px; Use Basic Configuration - true + false @@ -1036,7 +1035,7 @@ margin-top: -1px; 100 - 800 + 5000 400 @@ -2296,7 +2295,7 @@ border-radius: 5; 100 - 800 + 5000 400 @@ -9504,7 +9503,7 @@ border-radius: 5; - + 0 @@ -9584,7 +9583,7 @@ response (deg/s) - + 0 @@ -9637,7 +9636,7 @@ response (deg/s) - + 0 @@ -9703,58 +9702,6 @@ response (deg) - - - - - 0 - 0 - - - - - 0 - 22 - - - - - 16777215 - 22 - - - - Qt::StrongFocus - - - <html><head/><body><p>This sets the maximum deg your vehicle will tilt at full stick input when in Attitude mode.</p></body></html> - - - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 0 - - - 180.000000000000000 - - - 1.000000000000000 - - - - objname:StabilizationSettingsBankX - fieldname:YawMax - haslimits:no - scale:1 - buttongroup:6,20 - - - - @@ -10306,7 +10253,7 @@ border-radius: 5; - + 0 @@ -10909,7 +10856,7 @@ border-radius: 5; - + 0 @@ -10964,7 +10911,7 @@ border-radius: 5; - + 0 @@ -11017,7 +10964,7 @@ border-radius: 5; - + 0 @@ -11070,7 +11017,7 @@ border-radius: 5; - + 0 @@ -16169,7 +16116,7 @@ border-radius: 5; - + 0 @@ -16229,56 +16176,6 @@ border-radius: 5; - - - - - 0 - 0 - - - - - 85 - 22 - - - - - 16777215 - 22 - - - - Qt::StrongFocus - - - <html><head/><body><p>This adjusts how much leveling stability is set into Attitude mode (outer loop). Too much will make your vehicle oscillate in Attitude Mode.</p></body></html> - - - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - 0.100000000000000 - - - - objname:StabilizationSettingsBankX - fieldname:YawPI - element:Kp - haslimits:no - scale:1 - buttongroup:5,20 - - - - @@ -16904,606 +16801,6 @@ border-radius: 5; - - - - - 0 - 0 - - - - - 0 - 20 - - - - - - - - - 255 - 255 - 255 - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - 58 - 58 - 58 - - - - - - - 48 - 48 - 48 - - - - - - - 19 - 19 - 19 - - - - - - - 26 - 26 - 26 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - 0 - 0 - 0 - - - - - - - 19 - 19 - 19 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 255 - 255 - 255 - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - 58 - 58 - 58 - - - - - - - 48 - 48 - 48 - - - - - - - 19 - 19 - 19 - - - - - - - 26 - 26 - 26 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - 0 - 0 - 0 - - - - - - - 19 - 19 - 19 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 255 - 255 - 255 - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - 58 - 58 - 58 - - - - - - - 48 - 48 - 48 - - - - - - - 19 - 19 - 19 - - - - - - - 26 - 26 - 26 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - - - 74 - 74 - 74 - - - - - 36 - 36 - 36 - - - - - - - - - 0 - 0 - 0 - - - - - - - 39 - 39 - 39 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 75 - true - - - - false - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; - - - Yaw - - - Qt::AlignCenter - - - - - - - - 0 - 0 - - - - - 0 - 22 - - - - - 16777215 - 22 - - - - Qt::StrongFocus - - - <html><head/><body><p>This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Attitude Mode. Adding Ki in Attitude when Ki is present in Rate is not recommended.</p></body></html> - - - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - 0.100000000000000 - - - - objname:StabilizationSettingsBankX - fieldname:YawPI - element:Ki - haslimits:no - scale:1 - buttongroup:5,20 - - - - @@ -27459,15 +26756,14 @@ Useful if you have accidentally changed some settings. scrollArea_2 advancedResponsivenessCheckBox pushButton_3 - rateRollKp_3 - ratePitchKp_4 - rateYawKp_3 - rateRollKi_3 - ratePitchKi_4 - rateYawKi_3 - rateRollILimit_3 - ratePitchILimit_4 - rateYawILimit_3 + attitudeRollResponse + attitudePitchResponse + rateRollResponse + ratePitchResponse + rateYawResponse + maxRateRollLimit + maxRatePitchLimit + maxRateYawLimit enableThrustPIDScalingCheckBox ThrustPIDSource ThrustPIDTarget @@ -27488,10 +26784,8 @@ Useful if you have accidentally changed some settings. pushButton_2 AttitudeRollKp AttitudePitchKp_2 - AttitudeYawKp AttitudeRollKi AttitudePitchKi_2 - AttitudeYawKi enableThrustPIDScalingCheckBox_2 pushButton_13 realTimeUpdates_12 diff --git a/ground/gcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/gcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 29c942416..600e72e68 100644 --- a/ground/gcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/gcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -776,30 +776,30 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch case VehicleConfigurationSource::MULTI_ROTOR_HEXA: case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: case VehicleConfigurationSource::MULTI_ROTOR_HEXA_X: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 100); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); break; case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - mSettings->setMixerValueRoll((qint8) 50); - mSettings->setMixerValuePitch((qint8) 50); - mSettings->setMixerValueYaw((qint8) 50); + mSettings->setMixerValueRoll((qint8)50); + mSettings->setMixerValuePitch((qint8)50); + mSettings->setMixerValueYaw((qint8)50); break; case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 50); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)50); break; case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 50); - mSettings->setMixerValueYaw((qint8) 66); + mSettings->setMixerValueRoll((qint8)86); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); break; case VehicleConfigurationSource::MULTI_ROTOR_OCTO: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_X: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 100); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); break; case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: @@ -811,9 +811,9 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch break; } case VehicleConfigurationSource::VEHICLE_FIXEDWING: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 100); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); maxThrottle = 1; break; case VehicleConfigurationSource::VEHICLE_HELI: @@ -822,22 +822,22 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch { switch (m_configSource->getVehicleSubType()) { case VehicleConfigurationSource::GROUNDVEHICLE_MOTORCYCLE: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 100); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); maxThrottle = 1; break; case VehicleConfigurationSource::GROUNDVEHICLE_CAR: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 100); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); maxThrottle = 1; minThrottle = 0; break; case VehicleConfigurationSource::GROUNDVEHICLE_DIFFERENTIAL: - mSettings->setMixerValueRoll((qint8) 100); - mSettings->setMixerValuePitch((qint8) 100); - mSettings->setMixerValueYaw((qint8) 100); + mSettings->setMixerValueRoll((qint8)100); + mSettings->setMixerValuePitch((qint8)100); + mSettings->setMixerValueYaw((qint8)100); maxThrottle = 0.8; minThrottle = 0; break; diff --git a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp index fd3933144..6b566af28 100644 --- a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp +++ b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp @@ -496,12 +496,18 @@ void generateIndexedProperty(Context &ctxt, FieldContext &fieldCtxt) for (int elementIndex = 0; elementIndex < fieldCtxt.field->numElements; elementIndex++) { QString elementName = fieldCtxt.field->elementNames[elementIndex]; + QString sep; + if (fieldCtxt.propName.right(1)[0].isDigit() && elementName[0].isDigit()) { + info(ctxt.object, "Property \"" + fieldCtxt.propName + "\" and element \"" + elementName + "\" have digit conflict, consider fixing it."); + sep = "_"; + } + FieldContext elementCtxt; elementCtxt.field = fieldCtxt.field; elementCtxt.fieldName = fieldCtxt.fieldName + "_" + elementName; elementCtxt.fieldType = fieldCtxt.fieldType; - elementCtxt.propName = fieldCtxt.propName + elementName; - elementCtxt.ucPropName = fieldCtxt.ucPropName + elementName; + elementCtxt.propName = fieldCtxt.propName + sep + elementName; + elementCtxt.ucPropName = fieldCtxt.ucPropName + sep + elementName; elementCtxt.propType = fieldCtxt.propType; elementCtxt.propRefType = fieldCtxt.propRefType; // deprecation @@ -621,24 +627,22 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo *object) // field context FieldContext fieldCtxt; - fieldCtxt.field = field; + fieldCtxt.field = field; // field properties - fieldCtxt.fieldName = field->name; - fieldCtxt.fieldType = fieldTypeStrCPP(field->type); + fieldCtxt.fieldName = field->name; + fieldCtxt.fieldType = fieldTypeStrCPP(field->type); - fieldCtxt.ucPropName = toPropertyName(field->name); - fieldCtxt.propName = toLowerCamelCase(fieldCtxt.ucPropName); - fieldCtxt.propType = fieldCtxt.fieldType; + fieldCtxt.ucPropName = toPropertyName(field->name); + fieldCtxt.propName = toLowerCamelCase(fieldCtxt.ucPropName); + fieldCtxt.propType = fieldCtxt.fieldType; if (field->type == FIELDTYPE_INT8) { fieldCtxt.propType = fieldTypeStrCPP(FIELDTYPE_INT16); - } - else if (field->type == FIELDTYPE_UINT8) { + } else if (field->type == FIELDTYPE_UINT8) { fieldCtxt.propType = fieldTypeStrCPP(FIELDTYPE_UINT16); - } - else if (field->type == FIELDTYPE_ENUM) { + } else if (field->type == FIELDTYPE_ENUM) { QString enumClassName = object->name + "_" + fieldCtxt.ucPropName; - fieldCtxt.propType = enumClassName + "::Enum"; + fieldCtxt.propType = enumClassName + "::Enum"; } // reference type fieldCtxt.propRefType = fieldCtxt.propType; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f5fc9645f..f79cb6496 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,9 +2,8 @@ Selection of optional hardware configurations. - - - + + @@ -15,9 +14,8 @@ - - - + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 93565d66c..2d2a2def3 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,EX.Bus,HoTT,SRXL,GCS,OPLink,None" defaultvalue="None"/> Monitors which receiver channels have been active within the last second.