void BatteryMeasure_update(float *MeasuredVoltage) { electrical_setup(); //float MeasuredVoltage; unsigned char lsb, msb; lsb = i2c_smbus_read_byte_data(BatteryI2C_fd, 0x37); msb = i2c_smbus_read_byte_data(BatteryI2C_fd, 0x38); int raw_voltage = (lsb >> 6) | (msb << 2); // we know from spec sheet that ADCIN0 has no prescaler // so that the max voltage range is 1.5 volt // multiply by ten to get decivolts //from raw measurement we got quite a linear response //9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923 //leading to our 0.13595166 magic number for decivolts conversion //electrical.vsupply = raw_voltage*0.13595166; *MeasuredVoltage = raw_voltage*0.13595166; }
void electrical_init(void) { // First we try to kill the program.elf if it is running (done here because initializes first) system("killall -9 program.elf"); // Initialize 12c device for power fd = open( "/dev/i2c-1", O_RDWR ); if ( ioctl( fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) { fprintf( stderr, "Failed to set slave address: %m\n" ); } electrical_setup(); electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY; }
void BatteryMeasure_init(void) { // First we try to kill the program.elf and its respawner if it is running (done here because initializes first) // int ret = system("killall -9 program.elf.respawner.sh; killall -9 program.elf"); //(void) ret; // Initialize 12c device for power BatteryI2C_fd = open( "/dev/i2c-1", O_RDWR ); if ( ioctl( BatteryI2C_fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) { fprintf( stderr, "Failed to set slave address: %m\n" ); } electrical_setup(); //electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY; }
void electrical_periodic(void) { electrical_setup(); unsigned char lsb, msb; lsb = i2c_smbus_read_byte_data(fd, 0x37); msb = i2c_smbus_read_byte_data(fd, 0x38); int raw_voltage = (lsb >> 6) | (msb << 2); // we know from spec sheet that ADCIN0 has no prescaler // so that the max voltage range is 1.5 volt // multiply by ten to get decivolts //from raw measurement we got quite a lineair response //9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923 //leading to our 0.13595166 magic number for decivolts conversion electrical.vsupply = raw_voltage*0.13595166; /* * Superellipse: abs(x/a)^n + abs(y/b)^n = 1 * with a = 1 * b = mA at full throttle * n = 1.2 This defines nonlinearity (1 = linear) * x = throttle * y = current * * define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2 */ float b = (float)MILLIAMP_AT_FULL_THROTTLE; float x = ((float)commands[COMMAND_CURRENT_ESTIMATION]) / ((float)MAX_PPRZ); /* electrical.current y = ( b^n - (b* x/a)^n )^1/n * a=1, n = electrical_priv.nonlin_factor */ electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); }