void hello(void) {
if (test_silent)
return;
nx_sound_freq(1000, 100);
nx_systick_wait_ms(50);
nx_sound_freq(2000, 100);
nx_systick_wait_ms(900);
}
void tests_usb_hardcore(void) {
int i, lng;
char buffer[NX_USB_PACKET_SIZE];
hello();
nx_systick_wait_ms(6000);
nx_usb_read((U8 *)(&buffer), NX_USB_PACKET_SIZE);
for (i = 0 ; i < 1800 ; i++) {
if ( (lng = nx_usb_data_read()) > 0) {
if (streq(buffer, "halt")) {
break;
}
nx_usb_read((U8 *)(&buffer), NX_USB_PACKET_SIZE);
}
nx_usb_write((U8 *)"TEST", 5);
}
goodbye();
}
void tests_sensors(void) {
U32 i, sensor;
const U32 display_seconds = 15;
hello();
for (sensor=0; sensor<NXT_N_SENSORS; sensor++) {
nx_sensors_analog_enable(sensor);
}
for (i=0; i<(display_seconds*4); i++) {
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("- Sensor info -\n"
"----------------\n");
for (sensor=0; sensor<NXT_N_SENSORS; sensor++){
nx_display_string("Port ");
nx_display_uint(sensor);
nx_display_string(": ");
nx_display_uint(nx_sensors_analog_get(sensor));
nx_display_end_line();
}
nx_systick_wait_ms(250);
}
for (sensor=0; sensor<NXT_N_SENSORS; sensor++) {
nx_sensors_analog_disable(sensor);
}
goodbye();
}
/**
* Cette fonction affiche les valeurs des capteurs
*/
void vDisplaySensorsValues() {
U8 radar;
U32 contact;
U32 light;
while(TRUE) {
nx_display_clear();
nx_display_cursor_set_pos(0, 5);
nx_display_string("Touch");
contact=nx_sensors_analog_get(TOUCH_SENSOR);
nx_display_cursor_set_pos(7, 5);
nx_display_uint(contact);
nx_display_cursor_set_pos(0, 6);
nx_display_string("Radar");
radar=nx_radar_read_distance(RADAR_SENSOR, 0);
nx_display_cursor_set_pos(7, 6);
nx_display_uint(radar);
nx_display_cursor_set_pos(0, 7);
nx_display_string("Light");
light=nx_sensors_analog_get(LIGHT_SENSOR);
nx_display_cursor_set_pos(7, 7);
nx_display_uint(light);
nx_systick_wait_ms(50);
}
}
void tests_fs(void) {
hello();
fs_test_infos();
nx_systick_wait_ms(2000);
fs_test_dump();
goodbye();
}
//teste si le robot est sur un drapeau
void vForwardUntilWhite(U32 *last_left_counter, U32 *last_right_counter) {
if (nx_sensors_analog_get(LIGHT_SENSOR)<500) {
//arrete les moteurs
nx_motors_stop(RIGHT_MOTOR, TRUE);
nx_motors_stop(LEFT_MOTOR, TRUE);
//envoi message
S8 data[MSG_SIZE];
data[0]=2;
data[1]=iPositionX;
data[2]=iPositionY;
data[3]=iOrientation;
nx_bt_stream_write((U8 *)data, MSG_SIZE);
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
vPlaySond();
//attends 3 secondes
nx_systick_wait_ms(3000);
//avance pour sortir du drapeau
vForwardStop(last_left_counter, last_right_counter,150); //changer ici en focntion de la taille du drapeau
//avance
nx_motors_rotate(LEFT_MOTOR,MEDIUM_SPEED);
nx_motors_rotate(RIGHT_MOTOR, MEDIUM_SPEED);
}
}
void main(void) {
char *entries[] = {"Browse", "Sysinfo", "Settings", "Format", "Item5", "Item6", "Item7", "Halt", NULL};
gui_text_menu_t menu;
U8 res;
menu.entries = entries;
menu.title = "Home menu";
menu.active_mark = "> ";
while (TRUE) {
res = nx_gui_text_menu(menu);
nx_display_end_line();
switch (res) {
case 7:
nx_display_clear();
nx_display_string(">> Halting...");
nx_systick_wait_ms(1000);
return;
break;
default:
nx_display_clear();
nx_display_string("You pressed:\n");
nx_display_string(entries[res]);
nx_display_end_line();
nx_display_string("\nOk to go back");
while (nx_avr_get_button() != BUTTON_OK);
break;
}
}
}
void handle_cmd(void)
{
S8 speed[MSG_SIZE];
//reception du message
nx_bt_stream_read((U8 *)speed, MSG_SIZE);
while (!quit && nx_bt_stream_data_read() < 1) {
detect_obstacle();
vForwardUntilWall(&pos1Global,&pos2Global);
vForwardUntilWhite(&pos1Global,&pos2Global);
nx_systick_wait_ms(10);
nx_display_cursor_set_pos(0, 5);
nx_display_string("boucle");
}
if (speed[0] > 100 || speed[0] < -100
|| speed[1] > 100 || speed[1] < -100)
speed[0] = speed[1] = 0;
nx_motors_rotate_time(LEFT_MOTOR, speed[0], MOVE_TIME, FALSE);
nx_motors_rotate_time(RIGHT_MOTOR, speed[1], MOVE_TIME, FALSE);
return;
}
fs_err_t usb_recv_to(fs_fd_t fd) {
U8 buf[RCMD_BUF_LEN];
fs_err_t err;
size_t i;
do {
memset(buf, 0, RCMD_BUF_LEN);
usb_readline(buf);
if (!*buf) {
continue;
}
for (i=0; i<strlen((char *)buf); i++) {
err = nx_fs_write(fd, buf[i]);
if (err != FS_ERR_NO_ERROR) {
return err;
}
}
err = nx_fs_write(fd, (U8)'\n');
if (err != FS_ERR_NO_ERROR) {
return err;
}
nx_rcmd_do((char *)buf);
nx_systick_wait_ms(50);
} while (!streq((char *)buf, "end"));
return FS_ERR_NO_ERROR;
}
void tests_ht_compass(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Test of compass\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n\n");
ht_compass_init(sensor);
if( ! ht_compass_detect(sensor) ) {
nx_display_string("No compass!\n");
goodbye();
return;
}
ht_compass_info(sensor);
while(nx_avr_get_button() != BUTTON_OK) {
nx_display_cursor_set_pos(9, 6);
nx_display_string(" ");
nx_display_cursor_set_pos(9, 6);
nx_display_uint( ht_compass_read_heading(sensor) );
nx_systick_wait_ms(100);
}
ht_compass_close(sensor);
goodbye();
}
//est moins precise que vForwardStop
void vForward(U32 distance) {
float angle;
angle=360*distance/(PI*55);
nx_motors_rotate_angle(LEFT_MOTOR, HIGH_SPEED, (U32)angle, TRUE);
nx_motors_rotate_angle(RIGHT_MOTOR, HIGH_SPEED, (U32)angle, TRUE);
nx_systick_wait_ms(500+distance/200*1000);
}
static nx__ram_function bool nx__efc_do_write(U32 page) {
U32 ret;
NX_ASSERT(page < EFC_PAGES);
// turn off systick callbacks
nx_systick_suspend();
// sync
nx_systick_wait_ms(1);
// manually call avr update
nx__avr_fast_update();
// wait for avr update
while (!nx__twi_ready());
// disable interrupts
nx_interrupts_disable();
/* Trigger the flash write command. */
*AT91C_MC_FCR = EFC_WRITE + ((page & 0x000003FF) << 8);
/* Wait for the command to complete. */
do {
ret = *AT91C_MC_FSR;
} while (!(ret & AT91C_MC_FRDY));
// reenable interrupts
nx_interrupts_enable();
// sync
nx_systick_wait_ms(1);
// turn on systick callbacks
nx_systick_resume();
/* Check the command result by reading the status register
* only once to avoid the bits being cleared.
*/
if (ret & AT91C_MC_LOCKE || ret & AT91C_MC_PROGE)
return FALSE;
return TRUE;
}
/**
* Arret du robot
*/
void die(void)
{
nx_display_string("dying...\n");
nx_motors_stop(LEFT_MOTOR, TRUE);
nx_motors_stop(RIGHT_MOTOR, TRUE);
nx_radar_close(RADAR_SENSOR);
bt_die();
nx_systick_wait_ms(2000);
nx_core_halt();
}
/* TODO: figure out if its faster or not to retrieve the
* data or just a pointer, relative to the data retrieval
* mechanism from the flash.
*/
void nx__efc_read_page(U32 page, U32 *data) {
U8 i;
NX_ASSERT(page < EFC_PAGES);
nx_systick_wait_ms(EFC_THROTTLE_TIMER);
for (i=0; i<EFC_PAGE_WORDS; i++) {
data[i] = FLASH_BASE_PTR[page*EFC_PAGE_WORDS+i];
}
}
void main(void)
{
init();
nx_systick_wait_ms(5000);
/*
vDisplaySensorsValues();
U32 pos1=0;
U32 pos2=0;
vForwardStop(&pos1,&pos2,500);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnLeft(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,500);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnLeft(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
*/
nx_display_cursor_set_pos(0, 5);
nx_display_string("en attente");
while (!quit) {
if (!nx_bt_stream_opened() || nx_bt_connection_pending())
bt_wait_connection();
else
{
nx_sound_freq(DO, 200);
nx_sound_freq(DO, 200);
nx_sound_freq(DO, 200);
handle_cmd();
}
}
die();
}
//recule tant que la position voulue n'est pas atteinte
void vBackStop(U32 *last_left_counter, U32 *last_right_counter, int distance) {
U32 pos1=*last_left_counter;
U32 pos2=*last_right_counter;
float fAngle;
fAngle=360*distance/(PI*55);
U32 uAngle=(U32)fAngle;
nx_motors_rotate(LEFT_MOTOR, -80);
nx_motors_rotate(RIGHT_MOTOR, -80);
while(pos1>*last_left_counter-uAngle && pos2>*last_right_counter-uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
if (pos1<=*last_left_counter-uAngle) {
nx_motors_stop(LEFT_MOTOR, TRUE);
while(pos2>*last_right_counter-uAngle) {
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(RIGHT_MOTOR, TRUE);
}
else{
nx_motors_stop(RIGHT_MOTOR, TRUE);
while(pos1>*last_left_counter-uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(LEFT_MOTOR, TRUE);
}
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
}
void main (void) {
nx_systick_install_scheduler(security_hook);
nx_rs485_init();
while (1) {
lock = 1;
nx_rs485_send(out_buffer, sizeof(out_buffer), send_callback);
while (lock) {
nx_systick_wait_ms(1000);
}
}
}
/**
* Cette fonction permet de faire tourner le robot vers la gauche
*/
void vTurnLeft(U32 *last_left_counter, U32 *last_right_counter) {
U32 pos1=*last_left_counter;
U32 pos2=*last_right_counter;
float fAngle;
fAngle=360*PI*WHEEL_SPACING/(4*PI*55);
U32 uAngle=(U32)fAngle;
iOrientation=(iOrientation-1)%4;
nx_motors_rotate(LEFT_MOTOR, -80);
nx_motors_rotate(RIGHT_MOTOR, 80);
while(pos1>*(last_left_counter-uAngle) && pos2<(*last_right_counter+uAngle)) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
if (pos1>=*last_left_counter-uAngle) {
nx_motors_stop(LEFT_MOTOR, TRUE);
while(pos2<*last_right_counter+uAngle) {
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(RIGHT_MOTOR, TRUE);
}
else{
nx_motors_stop(RIGHT_MOTOR, TRUE);
while(pos1>*last_left_counter-uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(LEFT_MOTOR, TRUE);
}
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
}
void tests_ht_color(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Test of ht_color\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n");
ht_color_init(sensor);
if( ! ht_color_detect(sensor) ) {
nx_display_string("No color!\n");
goodbye();
return;
}
ht_color_info(sensor);
ht_color_values values;
while(nx_avr_get_button() != BUTTON_OK) {
if ( ! ht_color_read_values(sensor, &values) ) {
nx_display_string("Error reading!");
break;
}
nx_display_cursor_set_pos(0, 5);
nx_display_string("#: ");
nx_display_int(values.colornum);
nx_display_string(" ");
nx_display_cursor_set_pos(9, 5);
nx_display_string("R: ");
nx_display_int(values.redval);
nx_display_string(" ");
nx_display_cursor_set_pos(0, 6);
nx_display_string("G: ");
nx_display_int(values.greenval);
nx_display_string(" ");
nx_display_cursor_set_pos(9, 6);
nx_display_string("B: ");
nx_display_int(values.blueval);
nx_display_string(" ");
nx_display_end_line();
nx_systick_wait_ms(100);
}
ht_color_close(sensor);
goodbye();
}
void tests_motor(void) {
hello();
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("--- AVR test ---\n"
"----------------\n");
nx__avr_set_motor(0, 80, 0);
nx_systick_wait_ms(1000);
nx__avr_set_motor(0, -80, 0);
nx_systick_wait_ms(1000);
nx__avr_set_motor(0, 80, 0);
nx_systick_wait_ms(1000);
nx__avr_set_motor(0, 0, 1);
nx_systick_wait_ms(200);
goodbye();
}
void tests_sysinfo(void) {
U32 i;
U32 t = 0;
const U32 display_seconds = 15;
U8 avr_major, avr_minor;
hello();
nx_avr_get_version(&avr_major, &avr_minor);
for (i=0; i<(display_seconds*4); i++) {
if (i % 4 == 0)
t = nx_systick_get_ms();
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("- System info -\n"
"----------------\n");
nx_display_string("Time : ");
nx_display_uint(t);
nx_display_end_line();
nx_display_string("Boot from ");
if (NX_BOOT_FROM_SAMBA)
nx_display_string("SAM-BA");
else if (NX_BOOT_FROM_ENH_FW)
nx_display_string("ENH-FW");
else
nx_display_string("ROM");
nx_display_end_line();
nx_display_string("Free RAM: ");
nx_display_uint(NX_USERSPACE_SIZE);
nx_display_end_line();
nx_display_string("Buttons: ");
nx_display_uint(nx_avr_get_button());
nx_display_end_line();
nx_display_string("Battery: ");
nx_display_uint(nx_avr_get_battery_voltage());
nx_display_string(" mV");
nx_display_end_line();
nx_systick_wait_ms(250);
}
goodbye();
}
static void core_init(void) {
nx__aic_init();
nx_interrupts_enable();
nx__systick_init();
nx__sound_init();
nx__avr_init();
nx__motors_init();
nx__lcd_init();
nx__display_init();
nx__sensors_init();
nx__usb_init();
nx_i2c_init(); // TODO: should be nx__i2c_init().
/* Delay a little post-init, to let all the drivers settle down. */
nx_systick_wait_ms(100);
}
/* Write one page at the given page number in the flash.
* Use interrupt-driven flash programming ?
*/
__attribute__((noinline)) bool nx__efc_write_page(U32 *data, U32 page) {
U8 i;
NX_ASSERT(page < EFC_PAGES);
/* Wait for the flash to be ready. */
nx__efc_wait_for_flash();
nx_systick_wait_ms(EFC_THROTTLE_TIMER);
/* Write the page data to the flash in-memory mapping. */
for (i=0 ; i<EFC_PAGE_WORDS ; i++) {
FLASH_BASE_PTR[i+page*EFC_PAGE_WORDS] = data[i];
}
return nx__efc_do_write(page);
}
bool ht_color_perform_calibration(U32 sensor, U8 mode) {
U8 str[1] = { mode };
switch (mode) {
case HT_COLOR_CAL_WHITEPOINT:
case HT_COLOR_CAL_BLACKPOINT:
return (nx_i2c_memory_write(sensor, HT_COLOR_COMMAND, str, 1) == I2C_ERR_OK);
break;
default:
nx_display_string("Unknown Cal. Command!\n");
nx_systick_wait_ms(1000);
return FALSE;
break;
}
}
/** Reads a value at 'internal_address' in the memory unit on the
* given sensor port and returns it in the given buffer. Size is the
* expected returned data size in bytes. The buffer 'buf' should be
* pre-allocated by the caller.
*/
i2c_txn_err nx_i2c_memory_read(U32 sensor, U8 internal_address,
U8 *buf, U32 size) {
i2c_txn_err err;
if (!buf || !size || size >= I2C_MAX_DATA_SIZE)
return I2C_ERR_DATA;
err = nx_i2c_start_transaction(sensor, TXN_MODE_READ,
&internal_address, 1, buf, size);
if (err != I2C_ERR_OK)
return err;
while (nx_i2c_busy(sensor))
nx_systick_wait_ms(NX_I2C_TXN_WAIT);
return nx_i2c_get_txn_status(sensor);
}
void tests_ht_accel(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Test of ht_accel\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n");
ht_accel_init(sensor);
if( ! ht_accel_detect(sensor) ) {
nx_display_string("No accel!\n");
goodbye();
return;
}
ht_accel_info(sensor);
ht_accel_values values;
while(nx_avr_get_button() != BUTTON_OK) {
nx_display_cursor_set_pos(3, 5);
if ( ! ht_accel_read_values(sensor, &values) ) {
nx_display_string("Error reading!");
break;
}
nx_display_string(" ");
nx_display_cursor_set_pos(3, 5);
nx_display_int( values.x );
nx_display_cursor_set_pos(3, 6);
nx_display_string(" ");
nx_display_cursor_set_pos(3, 6);
nx_display_int( values.y );
nx_display_cursor_set_pos(3, 7);
nx_display_string(" ");
nx_display_cursor_set_pos(3, 7);
nx_display_int( values.z );
nx_systick_wait_ms(100);
}
ht_accel_close(sensor);
goodbye();
}
//avance en reduisant la vitesse des moteurs au départ et à l'arrivée vers le point choisi
void vForwardStop(U32 *last_left_counter, U32 *last_right_counter, int distance) {
U32 pos=0;
U32 pos1=0;
U32 pos2=0;
float fAngle;
fAngle=360*distance/(PI*55);
U32 uAngle=(U32)fAngle;
nx_motors_rotate(LEFT_MOTOR, 70);
nx_motors_rotate(RIGHT_MOTOR, 70);
nx_systick_wait_ms(50);
nx_motors_rotate(LEFT_MOTOR, 80);
nx_motors_rotate(RIGHT_MOTOR, 80);
nx_systick_wait_ms(50);
nx_motors_rotate(LEFT_MOTOR, 90);
nx_motors_rotate(RIGHT_MOTOR, 90);
while(pos<*last_left_counter+uAngle-100) {
pos=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_rotate(LEFT_MOTOR, 80);
nx_motors_rotate(RIGHT_MOTOR, 80);
while(pos1<*last_left_counter+uAngle && pos2<*last_right_counter+uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
if (pos1>=*last_left_counter+uAngle) {
nx_motors_stop(LEFT_MOTOR, TRUE);
while(pos2<*last_right_counter+uAngle) {
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(RIGHT_MOTOR, TRUE);
}
else{
nx_motors_stop(RIGHT_MOTOR, TRUE);
while(pos1<*last_left_counter+uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(LEFT_MOTOR, TRUE);
}
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
}
static void usb_readline(U8 *buf) {
size_t len;
int i = 0;
nx_usb_read(buf, RCMD_BUF_LEN*sizeof(char));
for (i=0; i<10 && !nx_usb_data_read(); i++) {
nx_systick_wait_ms(200);
}
if (i >= 10) {
return;
}
len = nx_usb_data_read();
if (len+1 < RCMD_BUF_LEN) {
buf[len+1] = '\0';
} else {
buf[RCMD_BUF_LEN-1] = '\0';
}
}
void main(void) {
char *entries[] = {"Replay", "Record", "From USB", "Halt", NULL};
gui_text_menu_t menu;
U8 res;
/*
U32 nulldata[EFC_PAGE_WORDS] = {0};
for (res=128; res<140; res++)
nx__efc_write_page(nulldata, res);
*/
menu.entries = entries;
menu.title = "Tag route";
menu.active_mark = GUI_DEFAULT_TEXT_MARK;
while (TRUE) {
res = nx_gui_text_menu(menu);
switch (res) {
case 0:
replay(ROUTE_FILE);
break;
case 1:
record(ROUTE_FILE);
break;
case 2:
usb_recv();
break;
case 3:
return;
break;
default:
continue;
break;
}
nx_display_string("\nOk to go back");
while (nx_avr_get_button() != BUTTON_OK);
nx_systick_wait_ms(500);
}
}
/** Writes the given data of the given size at 'internal_address' on the
* remote memory unit.
*/
i2c_txn_err nx_i2c_memory_write(U32 sensor, U8 internal_address,
const U8 *data, U32 size) {
U8 buf[I2C_MAX_DATA_SIZE];
i2c_txn_err err;
if (!data || !size || size >= I2C_MAX_DATA_SIZE)
return I2C_ERR_DATA;
buf[0] = internal_address;
memcpy(buf+1, data, size);
err = nx_i2c_start_transaction(sensor, TXN_MODE_WRITE,
buf, size+1, 0, 0);
if (err != I2C_ERR_OK)
return err;
while (nx_i2c_busy(sensor))
nx_systick_wait_ms(NX_I2C_TXN_WAIT);
return nx_i2c_get_txn_status(sensor);
}
