Skip to content

Commit

Permalink
stop hardcoding motor count, use enum value
Browse files Browse the repository at this point in the history
  • Loading branch information
bkleiner committed Jul 5, 2023
1 parent a7394d2 commit 7cb9f4a
Show file tree
Hide file tree
Showing 6 changed files with 34 additions and 33 deletions.
9 changes: 4 additions & 5 deletions src/driver/serial_4way.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,10 @@
#define ARM_DEVICE_MATCH(device_id) (((device_id >> 8) > 0x00 && (device_id >> 8) < 0x90) && (device_id & 0xFF) == 0x06)

#define INTF_MODE_IDX 3 // index for DeviceInfostate
#define ESC_COUNT 4
#define BLHELI_SETTINGS_SIZE 0xFF

static serial_esc4way_device_t device;
static gpio_pins_t esc_pins[ESC_COUNT] = {PIN_NONE};
static gpio_pins_t esc_pins[MOTOR_PIN_MAX] = {PIN_NONE};

#define ESC_PIN (esc_pins[device.selected_esc])

Expand Down Expand Up @@ -138,7 +137,7 @@ uint8_t serial_4way_init() {
avr_bl_init_pin(pin);
}

return ESC_COUNT;
return MOTOR_PIN_MAX;
}

void serial_4way_release() {
Expand Down Expand Up @@ -214,7 +213,7 @@ serial_esc4way_ack_t serial_4way_send(uint8_t cmd, uint16_t addr, const uint8_t
}

case ESC4WAY_DEVICE_RESET: {
if (input[0] >= ESC_COUNT) {
if (input[0] >= MOTOR_PIN_MAX) {
return ESC4WAY_ACK_I_INVALID_CHANNEL;
}

Expand Down Expand Up @@ -246,7 +245,7 @@ serial_esc4way_ack_t serial_4way_send(uint8_t cmd, uint16_t addr, const uint8_t
case ESC4WAY_DEVICE_INIT_FLASH: {
device_set_disconnected();

if (input[0] >= ESC_COUNT) {
if (input[0] >= MOTOR_PIN_MAX) {
return ESC4WAY_ACK_I_INVALID_CHANNEL;
}

Expand Down
2 changes: 1 addition & 1 deletion src/flight/control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ typedef struct {

typedef struct {
uint8_t active;
float value[4];
float value[MOTOR_PIN_MAX];
} motor_test_t;

extern control_state_t state;
Expand Down
30 changes: 15 additions & 15 deletions src/flight/motor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ static float motord(float in, int x) {
return in + out;
}

static void motor_brushless_mixer_scale_calc(float mix[4]) {
static void motor_brushless_mixer_scale_calc(float mix[MOTOR_PIN_MAX]) {
// only enable once really in the air
if (flags.on_ground || !flags.in_air) {
return;
Expand All @@ -62,7 +62,7 @@ static void motor_brushless_mixer_scale_calc(float mix[4]) {
float mix_min = 1000.0f;
float mix_max = -1000.0f;

for (int i = 0; i < 4; i++) {
for (int i = 0; i < MOTOR_PIN_MAX; i++) {
if (mix[i] < mix_min)
mix_min = mix[i];
if (mix[i] > mix_max)
Expand All @@ -80,7 +80,7 @@ static void motor_brushless_mixer_scale_calc(float mix[4]) {
if (mix_range > 1.0f) {
const float scale = 1.0f / mix_range;

for (int i = 0; i < 4; i++)
for (int i = 0; i < MOTOR_PIN_MAX; i++)
mix[i] *= scale;

mix_min *= scale;
Expand All @@ -92,17 +92,17 @@ static void motor_brushless_mixer_scale_calc(float mix[4]) {
reduce_amount = mix_min;
}

for (int i = 0; i < 4; i++)
for (int i = 0; i < MOTOR_PIN_MAX; i++)
mix[i] -= reduce_amount;
}

static void motor_brushed_mixer_scale_calc(float mix[4]) {
static void motor_brushed_mixer_scale_calc(float mix[MOTOR_PIN_MAX]) {
// throttle reduction
float overthrottle = 0;
float underthrottle = 0.001f;
static float overthrottlefilt = 0;

for (int i = 0; i < 4; i++) {
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
if (mix[i] > overthrottle)
overthrottle = mix[i];
if (mix[i] < underthrottle)
Expand Down Expand Up @@ -134,7 +134,7 @@ static void motor_brushed_mixer_scale_calc(float mix[4]) {

if (overthrottle > 0) { // exceeding max motor thrust
float temp = overthrottle;
for (int i = 0; i < 4; i++) {
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
mix[i] -= temp;
}
}
Expand All @@ -143,7 +143,7 @@ static void motor_brushed_mixer_scale_calc(float mix[4]) {
if (flags.in_air == 1) {
float underthrottle = 0;

for (int i = 0; i < 4; i++) {
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
if (mix[i] < underthrottle)
underthrottle = mix[i];
}
Expand All @@ -153,20 +153,20 @@ static void motor_brushed_mixer_scale_calc(float mix[4]) {
underthrottle = -(float)MIX_THROTTLE_INCREASE_MAX;

if (underthrottle < 0.0f) {
for (int i = 0; i < 4; i++)
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++)
mix[i] -= underthrottle;
}
}
}

static void motor_mixer_scale_calc(float mix[4]) {
static void motor_mixer_scale_calc(float mix[MOTOR_PIN_MAX]) {
if (target.brushless) {
return motor_brushless_mixer_scale_calc(mix);
}
return motor_brushed_mixer_scale_calc(mix);
}

void motor_test_calc(bool motortest_usb, float mix[4]) {
void motor_test_calc(bool motortest_usb, float mix[MOTOR_PIN_MAX]) {
if (motortest_usb) {
// set mix according to values we got via usb
mix[MOTOR_FR] = motor_test.value[MOTOR_FR];
Expand Down Expand Up @@ -201,7 +201,7 @@ void motor_test_calc(bool motortest_usb, float mix[4]) {
}
}

void motor_mixer_calc(float mix[4]) {
void motor_mixer_calc(float mix[MOTOR_PIN_MAX]) {
if (profile.motor.invert_yaw) {
state.pidoutput.yaw = -state.pidoutput.yaw;
}
Expand Down Expand Up @@ -235,7 +235,7 @@ void motor_mixer_calc(float mix[4]) {
}

//********************************MOTOR OUTPUT***********************************************************
void motor_output_calc(float mix[4]) {
void motor_output_calc(float mix[MOTOR_PIN_MAX]) {
state.thrsum = 0; // reset throttle sum for voltage monitoring logic in main loop

// only apply digital idle if we are armed and not in motor test
Expand All @@ -246,7 +246,7 @@ void motor_output_calc(float mix[4]) {
}

// Begin for-loop to send motor commands
for (uint32_t i = 0; i < 4; i++) {
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
if (!flags.motortest_override) {
// use values as supplied in motor test mode
mix[i] = constrainf(mix[i], 0, 1);
Expand All @@ -260,5 +260,5 @@ void motor_output_calc(float mix[4]) {
}

// calculate throttle sum for voltage monitoring logic in main loop
state.thrsum = state.thrsum / 4;
state.thrsum = state.thrsum / MOTOR_PIN_MAX;
}
8 changes: 5 additions & 3 deletions src/flight/motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@

#include <stdbool.h>

void motor_test_calc(bool motortest_usb, float mix[4]);
void motor_mixer_calc(float mix[4]);
#include "core/project.h"

void motor_output_calc(float mix[4]);
void motor_test_calc(bool motortest_usb, float mix[MOTOR_PIN_MAX]);
void motor_mixer_calc(float mix[MOTOR_PIN_MAX]);

void motor_output_calc(float mix[MOTOR_PIN_MAX]);
6 changes: 3 additions & 3 deletions src/io/msp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,7 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd,
uint16_t data[8];
memset(data, 0, 8 * sizeof(uint16_t));

for (uint8_t i = 0; i < 4; i++) {
for (uint8_t i = 0; i < MOTOR_PIN_MAX; i++) {
if (motor_test.value[i] <= 0.0f) {
data[i] = 1000;
} else {
Expand Down Expand Up @@ -206,7 +206,7 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd,
uint16_t *values = (uint16_t *)(payload);

motor_test.active = 0;
for (uint8_t i = 0; i < 4; i++) {
for (uint8_t i = 0; i < MOTOR_PIN_MAX; i++) {
const uint16_t val = constrain(values[i], 1000, 2000);
if (val == 1000) {
motor_test.value[i] = MOTOR_OFF;
Expand Down Expand Up @@ -246,7 +246,7 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd,

default:
case MSP_PASSTHROUGH_ESC_4WAY: {
uint8_t data[1] = {4};
uint8_t data[1] = {MOTOR_PIN_MAX};
msp_send_reply(msp, magic, cmd, data, 1);

motor_test.active = 0;
Expand Down
12 changes: 6 additions & 6 deletions src/io/quic.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -419,7 +419,7 @@ static void process_motor_test(quic_t *quic, cbor_value_t *dec) {
break;

case QUIC_MOTOR_TEST_ENABLE:
for (uint8_t i = 0; i < 4; i++) {
for (uint8_t i = 0; i < MOTOR_PIN_MAX; i++) {
motor_test.value[i] = MOTOR_OFF;
}
motor_test.active = 1;
Expand All @@ -431,7 +431,7 @@ static void process_motor_test(quic_t *quic, cbor_value_t *dec) {
break;

case QUIC_MOTOR_TEST_DISABLE:
for (uint8_t i = 0; i < 4; i++) {
for (uint8_t i = 0; i < MOTOR_PIN_MAX; i++) {
motor_test.value[i] = MOTOR_OFF;
}
motor_test.active = 0;
Expand All @@ -443,11 +443,11 @@ static void process_motor_test(quic_t *quic, cbor_value_t *dec) {
break;

case QUIC_MOTOR_TEST_SET_VALUE: {
float values[4];
res = cbor_decode_float_array(dec, values, 4);
float values[MOTOR_PIN_MAX];
res = cbor_decode_float_array(dec, values, MOTOR_PIN_MAX);
check_cbor_error(QUIC_CMD_MOTOR);

for (uint8_t i = 0; i < 4; i++) {
for (uint8_t i = 0; i < MOTOR_PIN_MAX; i++) {
const float val = constrainf(values[i], 0.0f, 1.0f);
if (val == 0.0f) {
motor_test.value[i] = MOTOR_OFF;
Expand All @@ -456,7 +456,7 @@ static void process_motor_test(quic_t *quic, cbor_value_t *dec) {
}
}

res = cbor_encode_float_array(&enc, motor_test.value, 4);
res = cbor_encode_float_array(&enc, motor_test.value, MOTOR_PIN_MAX);
check_cbor_error(QUIC_CMD_MOTOR);

quic_send(quic, QUIC_CMD_MOTOR, QUIC_FLAG_NONE, encode_buffer, cbor_encoder_len(&enc));
Expand Down

0 comments on commit 7cb9f4a

Please sign in to comment.