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;
}
