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