uint8_t TWI_send_sweep(uint8_t pos) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(S_ADDRESS); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_SWEEP); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(pos); if(CONTROL != DATA_W) { Error(); return 0; } stop_bus(); return 1; }
uint8_t TWI_send_autonom_settings(uint8_t adr,uint8_t autoset) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_AUTONOM); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(autoset); if(CONTROL != DATA_W) { Error(); return 0; } stop_bus(); return 1; }
uint8_t TWI_send_something(uint8_t adr, uint8_t instruction, uint8_t packet) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(instruction); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(packet); if(CONTROL != DATA_W) { Error(); return 0; } stop_bus(); return 1; }
bool send_string(int adr, char str[]) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(adr); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(I_STRING); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } for(int i = 0; i < strlen(str); ++i) { set_data(str[i]); send_bus(); wait_for_bus(); } stop_bus(); return true; }
uint8_t TWI_send_float(uint8_t adr, float flo) { union Union_floatcast foo; foo.f = flo; start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_FLOAT); if(CONTROL != DATA_W) { Error(); return 0; } for(int i = 0; i < 4; ++i) { send_data_and_wait(foo.s[i]); } stop_bus(); return 1; }
uint8_t TWI_send_elevation(uint8_t elevation) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(ST_ADDRESS); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_ELEVATION); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(elevation); if(CONTROL != DATA_W) { Error(); return 0; } stop_bus(); return 1; }
bool send_string_fixed_length(int adr, uint8_t str[], int len) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(adr); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(I_STRING); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } for(int i = 0; i < len; ++i) { set_data(str[i]); send_bus(); wait_for_bus(); } stop_bus(); return true; }
uint8_t TWI_send_string_fixed_length(uint8_t adr, uint8_t str[], int length) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_STRING); if(CONTROL != DATA_W) { Error(); return 0; } for(int i = 0; i < length; ++i) { send_data_and_wait(str[i]); } stop_bus(); return 1; }
uint8_t TWI_send_string(uint8_t adr, char str[]) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_STRING); if(CONTROL != DATA_W) { Error(); return 0; } for(int i = 0; i < strlen(str); ++i) { send_data_and_wait(str[i]); } stop_bus(); return 1; }
bool send_sensors(int sens[8], int serv) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(G_ADRESS); //General Call, NO instruction byte, NO NACK control of data send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } for(int i=0; i < 8; ++i) //8 Sensors? { set_data(sens[i]); send_bus(); wait_for_bus(); } set_data(serv); send_bus(); wait_for_bus(); stop_bus(); return true; }
bool send_status(int adr) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(adr); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(I_STATUS); send_bus(); wait_for_bus(); if(CONTROL == ARBITRATION) { return true; } else if(CONTROL != DATA_W) { Error(); return false; } stop_bus(); return true; }
/** * Attempt to start driver on all available I2C busses. * * This function will return as soon as the first sensor * is detected on one of the available busses or if no * sensors are detected. * */ int start() { for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { if (start_bus(bus_options[i]) == PX4_OK) { return PX4_OK; } } return PX4_ERROR; }
bool send_command(int direction, int rot_elev, int speed) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(ST_ADRESS); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(I_COMMAND); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } set_data(direction); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } set_data(rot_elev); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } set_data(speed); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } stop_bus(); return true; }
/** * * Attempt to start driver on all available I2C busses. * * This function will return as soon as the first sensor * is detected on one of the available busses or if no * sensors are detected. * */ int start(uint8_t rotation) { if (g_dev != nullptr) { PX4_ERR("already started"); return PX4_ERROR; } for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) { return PX4_OK; } } return PX4_ERROR; }
uint8_t TWI_send_control_settings(uint8_t adr, uint8_t KP,uint8_t KI,uint8_t KD) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_SETTINGS); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(KP); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(KI); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(KD); if(CONTROL != DATA_W) { Error(); return 0; } stop_bus(); return 1; }
uint8_t TWI_send_command(uint8_t direction, uint8_t rotation, uint8_t speed) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(ST_ADDRESS); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_COMMAND); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(direction); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(rotation); if(CONTROL != DATA_W) { Error(); return 0; } send_data_and_wait(speed); if(CONTROL != DATA_W) { Error(); return 0; } stop_bus(); return 1; }
/** * Start the driver. * * This function call only returns once the driver * is either successfully up and running or failed to start. */ void start(enum HMC5883_BUS busid, enum Rotation rotation) { bool started = false; for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) { // this device is already started continue; } if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) { // not the one that is asked for continue; } started |= start_bus(bus_options[i], rotation); } if (!started) { exit(1); } }
/** * Start the driver. * * This function only returns if the driver is up and running * or failed to detect the sensor. */ void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external) { bool started = false; for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { if (busid == MPU9250_BUS_ALL && bus_options[i].dev != NULL) { // this device is already started continue; } if (busid != MPU9250_BUS_ALL && bus_options[i].busid != busid) { // not the one that is asked for continue; } started |= start_bus(bus_options[i], rotation, external); } exit(started ? 0 : 1); }
uint8_t TWI_send_sensors(uint8_t sens[8], uint8_t serv) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(G_ADDRESS);//General Call, NO instruction byte, NO NACK control of data if(CONTROL != ADDRESS_W) { Error(); return 0; } for(int i=0; i < 8; ++i) //7 Sensors? { send_data_and_wait(sens[i]); } send_data_and_wait(serv); stop_bus(); return 1; }
bool send_sweep(int pos) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(S_ADRESS); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(I_SWEEP); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } set_data(pos); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } stop_bus(); return true; }
bool send_settings() { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(C_ADRESS); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(I_SETTINGS); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } //set_data(settings) send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } stop_bus(); return true; }
bool send_something(int adr, int instruction, int packet) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return false; } set_data(adr); send_bus(); wait_for_bus(); if(CONTROL != ADRESS_W) { Error(); return false; } set_data(instruction); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } set_data(packet); send_bus(); wait_for_bus(); if(CONTROL != DATA_W) { Error(); return false; } stop_bus(); return true; }
uint8_t TWI_send_status(uint8_t adr) { start_bus(); wait_for_bus(); if(CONTROL != START) { Error(); return 0; } send_data_and_wait(adr); if(CONTROL != ADDRESS_W) { Error(); return 0; } send_data_and_wait(I_STATUS); if(CONTROL == ARBITRATION) { clear_int(); return 1; } stop_bus(); return 1; }