diff --git a/firmware/libsi/include/si/device/gc_controller.h b/firmware/libsi/include/si/device/gc_controller.h index 57c8d0d..a518f83 100644 --- a/firmware/libsi/include/si/device/gc_controller.h +++ b/firmware/libsi/include/si/device/gc_controller.h @@ -184,9 +184,11 @@ static inline void si_device_gc_input_valid(struct si_device_gc_controller *devi } /** - * Update the origin of the controller, based on an origin packet. + * Update the origin of the controller. + * + * If the origin data differs from the current origin, the "need origin" flag is set. * * @param device the device to set the wireless origin for - * @param origin_data pointer to the analog origin data (6 bytes) + * @param new_origin pointer to the new origin data (6 bytes) */ -void si_device_gc_set_wireless_origin(struct si_device_gc_controller *device, uint8_t *origin_data); \ No newline at end of file +void si_device_gc_set_origin(struct si_device_gc_controller *device, struct si_device_gc_input_state *new_origin); \ No newline at end of file diff --git a/firmware/libsi/src/device/gc_controller.c b/firmware/libsi/src/device/gc_controller.c index d04c789..39af68c 100644 --- a/firmware/libsi/src/device/gc_controller.c +++ b/firmware/libsi/src/device/gc_controller.c @@ -68,6 +68,22 @@ static uint8_t *pack_input_state(struct si_device_gc_input_state *src, uint8_t a return packed_state; } +// Set or clear the "need origin" flag in the input state and device info +static void set_need_origin(struct si_device_gc_controller *device, bool need_origin) +{ + // Always set the need_origin flag in the input state + device->input.buttons.need_origin = need_origin; + + // Also update the device info for non-wireless controllers + if (!si_device_gc_is_wireless(device)) { + if (need_origin) { + device->info[2] |= SI_NEED_ORIGIN; + } else { + device->info[2] &= ~SI_NEED_ORIGIN; + } + } +} + /** * Handle "info" commands. * @@ -118,8 +134,7 @@ static int handle_short_poll(const uint8_t *command, si_complete_cb_t callback, if (!si_device_gc_is_wireless(device)) { // Update the origin flags - device->input.buttons.need_origin = (device->info[2] & SI_NEED_ORIGIN) != 0; - device->input.buttons.use_origin = true; + device->input.buttons.use_origin = true; // Save the analog mode and motor state device->info[2] &= ~(SI_MOTOR_STATE_MASK | SI_ANALOG_MODE_MASK); @@ -154,13 +169,8 @@ static int handle_read_origin(const uint8_t *command, si_complete_cb_t callback, { struct si_device_gc_controller *device = (struct si_device_gc_controller *)context; - // Tell the host it no longer needs to fetch the origin - if (!si_device_gc_is_wireless(device)) { - device->info[2] &= ~SI_NEED_ORIGIN; - } - // Clear the "need origin" flag - device->input.buttons.need_origin = false; + set_need_origin(device, false); // Respond with the origin si_write_bytes((uint8_t *)(&device->origin), SI_CMD_GC_READ_ORIGIN_RESP, callback); @@ -186,10 +196,8 @@ static int handle_calibrate(const uint8_t *command, si_complete_cb_t callback, v device->origin.trigger_left = device->input.trigger_left; device->origin.trigger_right = device->input.trigger_right; - // Tell the host it no longer needs to fetch the origin - if (!si_device_gc_is_wireless(device)) { - device->info[2] &= ~SI_NEED_ORIGIN; - } + // Clear the "need origin" flag + set_need_origin(device, false); // Respond with the new origin si_write_bytes((uint8_t *)(&device->origin), SI_CMD_GC_CALIBRATE_RESP, callback); @@ -213,18 +221,20 @@ static int handle_long_poll(const uint8_t *command, si_complete_cb_t callback, v uint8_t analog_mode = command[1] & 0x07; uint8_t motor_state = command[2] & 0x03; - // Update the origin flags before responding - device->input.buttons.need_origin = (device->info[2] & SI_NEED_ORIGIN) != 0; - device->input.buttons.use_origin = true; - - // Save the analog mode and motor state if (!si_device_gc_is_wireless(device)) { + // Update the origin flags + device->input.buttons.use_origin = true; + + // Save the analog mode and motor state device->info[2] &= ~(SI_MOTOR_STATE_MASK | SI_ANALOG_MODE_MASK); device->info[2] |= motor_state << 3 | analog_mode; } + // If the input state is valid, use that for the response, otherwise use the origin + struct si_device_gc_input_state *state = device->input_valid ? &device->input : &device->origin; + // Respond with the current input state - si_write_bytes((uint8_t *)&device->input, SI_CMD_GC_LONG_POLL_RESP, callback); + si_write_bytes((uint8_t *)state, SI_CMD_GC_LONG_POLL_RESP, callback); return SI_CMD_GC_LONG_POLL_RESP; } @@ -296,10 +306,6 @@ void si_device_gc_init(struct si_device_gc_controller *device, uint8_t type) // Mark the input as valid initially device->input_valid = true; - // Request the origin on non-wireless controllers - if (!si_device_gc_is_wireless(device)) - device->info[2] = SI_NEED_ORIGIN; - // Register the SI commands handled by GameCube controllers si_command_register(SI_CMD_INFO, SI_CMD_INFO_LEN, handle_info, device); si_command_register(SI_CMD_RESET, SI_CMD_RESET_LEN, handle_reset, device); @@ -328,15 +334,15 @@ void si_device_gc_set_wireless_id(struct si_device_gc_controller *device, uint16 device->info[0] |= SI_GC_STANDARD | SI_WIRELESS_RECEIVED; } -void si_device_gc_set_wireless_origin(struct si_device_gc_controller *device, uint8_t *origin_data) +void si_device_gc_set_origin(struct si_device_gc_controller *device, struct si_device_gc_input_state *new_origin) { // Check if the origin packet is different from the last known origin - if (memcmp(&device->origin.stick_x, origin_data, 6) != 0) { + if (memcmp(&device->origin.stick_x, &new_origin->stick_x, 6) != 0) { // Update the origin state - memcpy(&device->origin.stick_x, origin_data, 6); + memcpy(&device->origin.stick_x, &new_origin->stick_x, 6); - // Set the "need origin" flag to true so the host knows to fetch the new origin - device->input.buttons.need_origin = true; + // Tell the host that new origin data is available + set_need_origin(device, true); } // Set the "has wireless origin" flag in the device info diff --git a/firmware/libsi/test/test_gc_controller.c b/firmware/libsi/test/test_gc_controller.c index 882cfc7..b42d355 100644 --- a/firmware/libsi/test/test_gc_controller.c +++ b/firmware/libsi/test/test_gc_controller.c @@ -40,7 +40,7 @@ static void test_gcc_info() simulate_command(&device, info_command); // Test device info response is as expected - uint8_t expected_response[] = {0x09, 0x00, 0x20}; + uint8_t expected_response[] = {0x09, 0x00, 0x00}; TEST_ASSERT_EQUAL(3, response_len); TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3); } @@ -52,18 +52,37 @@ static void test_gcc_info_after_read_origin() struct si_device_gc_controller device; si_device_gc_init(&device, SI_TYPE_GC | SI_GC_STANDARD); + // Set an origin + struct si_device_gc_input_state new_origin = { + .stick_x = 0x81, + .stick_y = 0x82, + .substick_x = 0x83, + .substick_y = 0x84, + .trigger_left = 0x11, + .trigger_right = 0x12, + }; + si_device_gc_set_origin(&device, &new_origin); + + // Send an info command + uint8_t info_command[] = {SI_CMD_INFO}; + simulate_command(&device, info_command); + + // Test device info response is as expected + uint8_t expected_response[] = {0x09, 0x00, 0x20}; + TEST_ASSERT_EQUAL(3, response_len); + TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3); + // Send a read origin command uint8_t read_origin_command[] = {SI_CMD_GC_READ_ORIGIN}; simulate_command(&device, read_origin_command); - // Send an info command - uint8_t info_command[] = {SI_CMD_INFO}; + // Send another info command simulate_command(&device, info_command); // Verify the "need_origin" flag is not present - uint8_t expected_response[] = {0x09, 0x00, 0x00}; + uint8_t expected_response_2[] = {0x09, 0x00, 0x00}; TEST_ASSERT_EQUAL(3, response_len); - TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3); + TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response_2, response_buf, 3); } // Test that "analog mode" and "motor state" are saved after a "poll" command @@ -283,8 +302,15 @@ static void test_set_wireless_origin(void) TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response_2, response_buf, 3); // Set the wireless origin - uint8_t origin_data[] = {0x85, 0x86, 0x87, 0x88, 0x11, 0x12}; - si_device_gc_set_wireless_origin(&device, origin_data); + struct si_device_gc_input_state origin = { + .stick_x = 0x85, + .stick_y = 0x86, + .substick_x = 0x87, + .substick_y = 0x88, + .trigger_left = 0x11, + .trigger_right = 0x12, + }; + si_device_gc_set_origin(&device, &origin); // Check the origin state is set correctly TEST_ASSERT_EQUAL_UINT8(0x85, device.origin.stick_x); diff --git a/firmware/receiver/src/main.c b/firmware/receiver/src/main.c index d5d140b..39bb5b3 100644 --- a/firmware/receiver/src/main.c +++ b/firmware/receiver/src/main.c @@ -218,14 +218,17 @@ static void handle_wavebird_packet(const uint8_t *packet) // // Copy the origin values from the packet - uint8_t new_origin[] = { - wavebird_origin_get_stick_x(message), wavebird_origin_get_stick_y(message), - wavebird_origin_get_substick_x(message), wavebird_origin_get_substick_y(message), - wavebird_origin_get_trigger_left(message), wavebird_origin_get_trigger_right(message), + struct si_device_gc_input_state new_origin = { + .stick_x = wavebird_origin_get_stick_x(message), + .stick_y = wavebird_origin_get_stick_y(message), + .substick_x = wavebird_origin_get_substick_x(message), + .substick_y = wavebird_origin_get_substick_y(message), + .trigger_left = wavebird_origin_get_trigger_left(message), + .trigger_right = wavebird_origin_get_trigger_right(message), }; // Update the origin state in the SI device - si_device_gc_set_wireless_origin(&si_device, new_origin); + si_device_gc_set_origin(&si_device, &new_origin); } }