diff options
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 52 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 92 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_bm.c | 18 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 6 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 2 |
5 files changed, 107 insertions, 63 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 51feac539..a865992ed 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -217,14 +217,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) exit(ERROR); } - /* initialize motors */ - if (OK != ar_init_motors(ardrone_write, gpios)) { - close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); - thread_running = false; - exit(ERROR); - } - /* Led animation */ int counter = 0; int led_counter = 0; @@ -245,6 +237,44 @@ int ardrone_interface_thread_main(int argc, char *argv[]) printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); + /* initialize motors */ + if (OK != ar_init_motors(ardrone_write, gpios)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + thread_running = false; + exit(ERROR); + } + + ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); + + + // XXX Re-done initialization to make sure it is accepted by the motors + // XXX should be removed after more testing, but no harm + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + ardrone_write = ardrone_open_uart(&uart_config_original); + + /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ + gpios = ar_multiplexing_init(); + + if (ardrone_write < 0) { + fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + thread_running = false; + exit(ERROR); + } + + /* initialize motors */ + if (OK != ar_init_motors(ardrone_write, gpios)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + thread_running = false; + exit(ERROR); + } + while (!thread_should_exit) { if (motor_test_mode) { @@ -267,7 +297,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } } - if (counter % 22 == 0) { + if (counter % 16 == 0) { if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); @@ -297,8 +327,8 @@ int ardrone_interface_thread_main(int argc, char *argv[]) if (led_counter == 12) led_counter = 0; } - /* run at approximately 100 Hz */ - usleep(1000); + /* run at approximately 200 Hz */ + usleep(5000); counter++; } diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 1e4b48c07..380ae9c15 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -55,6 +55,8 @@ typedef union { uint8_t bytes[2]; } motor_union_t; +#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */ + /** * @brief Generate the 8-byte motor set packet * @@ -217,69 +219,80 @@ int ar_init_motors(int ardrone_uart, int gpios) int i; int errcounter = 0; + + /* initial setup run */ for (i = 1; i < 5; ++i) { /* Initialize motors 1-4 */ - initbuf[3] = (uint8_t)i; errcounter += ar_select_motor(gpios, i); - write(ardrone_uart, &(initbuf[0]), 1); + write(ardrone_uart, &(initbuf[3]), 1); + fsync(ardrone_uart); - /* sleep 5 ms */ - usleep(5000); + /* + * write 0xE0 - request status + * receive one status byte + */ + write(ardrone_uart, &(initbuf[0]), 1); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*1); + /* + * write 0x91 - request checksum + * receive 120 status bytes + */ write(ardrone_uart, &(initbuf[1]), 1); - /* wait 50 ms */ - usleep(5000); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*120); + /* + * write 0xA1 - set status OK + * receive one status byte - should be A0 + * to confirm status is OK + */ write(ardrone_uart, &(initbuf[2]), 1); - /* wait 50 ms */ - usleep(50000); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*1); + /* + * set as motor i, where i = 1..4 + * receive nothing + */ + initbuf[3] = (uint8_t)i; write(ardrone_uart, &(initbuf[3]), 1); - /* wait 50 ms */ - usleep(50000); + fsync(ardrone_uart); + /* + * write 0x40 - check version + * receive 11 bytes encoding the version + */ write(ardrone_uart, &(initbuf[4]), 1); - /* wait 50 ms */ - usleep(50000); - - /* wait 5 ms */ - usleep(50000); - - /* enforce immediate write */ fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*11); ar_deselect_motor(gpios, i); - - usleep(500000); - /* sleep 500 ms */ + /* sleep 400 ms */ + usleep(400000); } /* start the multicast part */ errcounter += ar_select_motor(gpios, 0); - /* enable multicast */ - write(ardrone_uart, &(multicastbuf[0]), 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, &(multicastbuf[1]), 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, &(multicastbuf[2]), 1); - /* wait 1 ms */ - usleep(1000); + /* + * write six times A0 - enable broadcast + * receive nothing + */ + // for (int i = 0; i < sizeof(multicastbuf); i++) { + write(ardrone_uart, multicastbuf, 6); + fsync(ardrone_uart); - write(ardrone_uart, &(multicastbuf[3]), 1); - /* wait 1 ms */ - usleep(1000); + usleep(200000); - write(ardrone_uart, &(multicastbuf[4]), 1); - /* wait 1 ms */ - usleep(1000); + write(ardrone_uart, multicastbuf, 6); + fsync(ardrone_uart); - write(ardrone_uart, &(multicastbuf[5]), 1); + /* set motors to zero speed */ + ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); + fsync(ardrone_uart); if (errcounter != 0) { fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); @@ -316,6 +329,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); int ret; ret = write(ardrone_fd, buf, sizeof(buf)); + fsync(ardrone_fd); if (ret == sizeof(buf)) { return OK; } else { diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c index 29908ddd5..8face5aea 100644 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -147,18 +147,18 @@ void attitude_blackmagic_init(void) // }; static m_elem kal_gain[12 * 9] = { - 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, + 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0.01f, 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0.01f, 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0.01f, 0 , 0 , 0, 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, 0, 0 , 0 , 0, 0, 0, 0, 0, 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f + 0 , 0 , 0 , 0 , 0 , 0 , 0.4f , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4f , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4f }; //offset update only correct if not upside down. diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 1174a7524..0f6b6044f 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -87,9 +87,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul gyro_values.z = raw->gyro_rad_s[2]; float_vect3 accel_values; - accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 120; - accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 120; - accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 120; + accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; + accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; + accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; float_vect3 mag_values; mag_values.x = raw->magnetometer_ga[0]*1500.0f; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 205f88093..79f9c19e7 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -914,7 +914,7 @@ Sensors::ppm_poll() if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; /* throttle input */ - manual_control.throttle = (_rc.chan[_rc.function[THROTTLE]].scaled+1.0f)/2.0f; + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled/2.0f; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; |