int get_soc (void)
{
float upper_voltage = 14.7f;
float duty_cycle;
if (cc_state == FULLY_CHARGED)
return 100;
else if (cc_state == PULSED_CURRENT)
return 90;
else if (cc_state == VOLTAGE_SETTLE)
return 80;
else
{
batt_voltage = get_adc_voltage(ADC_BATT_V);
set_temperature_compensation( get_adc_voltage(ADC_TEMP), &upper_voltage, &duty_cycle );
set_current_compensation(BATTERY_AHR*0.1f, batt_current, &upper_voltage );
//Linear appoximation
return (int)( 80.0f*(batt_voltage - v_lvdc) / (upper_voltage - v_lvdc) );
}
}
int
HMC5883::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t z[2];
uint8_t y[2];
} hmc_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report));
if (ret != OK) {
perf_count(_comms_errors);
debug("data/status read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > 2048) ||
(abs(report.y) > 2048) ||
(abs(report.z) > 2048)) {
perf_count(_comms_errors);
goto out;
}
/* get measurements from the device */
new_report.temperature = 0;
if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) {
/*
if temperature compensation is enabled read the
temperature too.
We read the temperature every 10 samples to avoid
excessive I2C traffic
*/
if (_temperature_counter++ == 10) {
uint8_t raw_temperature[2];
_temperature_counter = 0;
ret = _interface->read(ADDR_TEMP_OUT_MSB,
raw_temperature, sizeof(raw_temperature));
if (ret == OK) {
int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
raw_temperature[1];
new_report.temperature = 25 + (temp16 / (16*8.0f));
_temperature_error_count = 0;
} else {
_temperature_error_count++;
if (_temperature_error_count == 10) {
/*
it probably really is an old HMC5883,
and can't do temperature. Disable it
*/
_temperature_error_count = 0;
debug("disabling temperature compensation");
set_temperature_compensation(0);
}
}
} else {
new_report.temperature = _last_report.temperature;
}
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = report.y;
new_report.y_raw = -report.x;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
xraw_f = -report.y;
yraw_f = report.x;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic < 0)
debug("ADVERT FAIL");
}
}
_last_report = new_report;
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_range();
}
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
unsigned dummy = arg;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
/* check calibration, but not actually return an error */
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
return calibrate(filp, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
debug("MAGIOCGEXTERNAL in main driver");
return _interface->ioctl(cmd, dummy);
case MAGIOCSTEMPCOMP:
return set_temperature_compensation(arg);
case DEVIOCGDEVICEID:
return _interface->ioctl(cmd, dummy);
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
__task void interrupted_charging (void)
{
float sol_voltage = 0.0f, sol_current = 0.0f, sol_power = 0.0f;
int pulse = 0;
int counter = 0;
//TODO: initialise hardware
init_pwm(40000);
init_adc();
set_mppt();
while (1)
{
batt_voltage = get_adc_voltage(ADC_BATT_V);
batt_current = get_adc_voltage(ADC_BATT_I);
sol_voltage = get_adc_voltage(ADC_SOL_V);
sol_current = get_adc_voltage(ADC_SOL_I);
sol_power = sol_voltage * sol_current;
temp = get_adc_voltage(ADC_TEMP);
set_temperature_compensation( temp, &v_high, &pulse_duty );
TRACE_INFO("1,%.0f,%i,%.2f,%.2f,%.2f,%.3f,%.1f,%.2F\n",
((double)get_time_t()), cc_state, batt_voltage, batt_current, sol_voltage, sol_current, duty_cycle, temp);
//Check for LVDC voltage
calc_lvdc(batt_current);
if ( batt_voltage < v_lvdc )
{
//Turn off outputs and screen.
//send os event to turn off
os_evt_set( UI_LVDC, ui_t );
}
switch (cc_state)
{
case BULK_CHARGING:
//Start charging battery with 0.1C current or as high as possible if 0.1C cannot be met
perturb_and_observe_cc_itter(BATTERY_AHR*0.1f);
if (++counter > 10)
set_current_compensation(BATTERY_AHR*0.1f, batt_current, &v_high);
if (sol_power < P_NIGHT_MODE)
{
if (set_mppt() < (P_NIGHT_MODE*1.2))
{
cc_state = NIGHT_MODE;
counter = 0;
TRACE_DEBUG("Starting Night Mode State\n");
break;
}
TRACE_DEBUG("Rescaned Power and Night mode not entered \n");
}
if (batt_voltage > v_high)
{
cc_state = VOLTAGE_SETTLE;
counter = 0;
TRACE_DEBUG("Starting Voltage Settle Charging State at V=%f \n", batt_voltage);
break;
}
os_dly_wait( P_AND_O_PERIOD );
break;
case VOLTAGE_SETTLE:
//Diable the MPPT Charging Circuit
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
if (batt_voltage < v_low)
{
GPIO_SetBits(GPIOB, GPIO_Pin_0);
cc_state = PULSED_CURRENT;
TRACE_DEBUG("Starting Pulsed Current Charging State at V=%f \n", batt_voltage);
break;
}
//5s delay
os_dly_wait(50);
break;
case PULSED_CURRENT:
counter++;
if (pulse)
{
//If greater than high period seconds, pulse is low. i.e sets up x% duty cycle
if ( counter > (300 * pulse_duty) )
pulse = 0;
//Enable MPPT hardware
GPIO_SetBits(GPIOB, GPIO_Pin_0);
//Run p&o itteration
perturb_and_observe_cc_itter(BATTERY_AHR*0.05f);
if ( counter > 10 )
{
//set_current_compensation(BATTERY_AHR*0.05f, batt_current);
if (sol_power < P_NIGHT_MODE)
{
if (set_mppt() < (P_NIGHT_MODE*1.2))
{
cc_state = NIGHT_MODE;
counter = 0;
TRACE_DEBUG("Starting Night Mode State\n");
break;
}
TRACE_DEBUG("Rescaned Power and Night mode not entered \n");
}
}
}
else
{
//If greater than 30s, reset.
if ( counter > 300)
{
counter = 0;
pulse = 1;
}
//Diable the MPPT Charging Circuit
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
}
if (batt_voltage > v_high)
{
counter = 0;
cc_state = FULLY_CHARGED;
TRACE_DEBUG("Starting Fully Charged Charging State at V=%f \n", batt_voltage);
break;
}
//100ms delay
os_dly_wait(10);
break;
case FULLY_CHARGED:
//Regulate so that Battery current = 0
//This means that the battery is no longer charged
//But the panel is used to power any connected load
//Diable the MPPT Charging Circuit
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
//Check when to start recharging again.
if (batt_voltage < v_restart)
{
cc_state = BULK_CHARGING;
TRACE_DEBUG("Restarting the Bulk Charging State \n");
break;
}
//5s delay
os_dly_wait(500);
break;
case NIGHT_MODE:
//Check every 5 minutes
//Counter used so that LVDC is still
//checked every 5 seconds
if (++counter > 2)
{
//Enable MPPT Circuit
GPIO_SetBits(GPIOB, GPIO_Pin_0);
if (set_mppt() > (P_NIGHT_MODE*1.2))
{
cc_state = BULK_CHARGING;
TRACE_DEBUG("Exiting Night Mode\n");
break;
}
//Diable the MPPT Charging Circuit
//GPIO_ResetBits(GPIOB, GPIO_Pin_0);
set_duty_cycle(100);
counter = 0;
}
//5 second wait time
os_dly_wait(500);
break;
default:
cc_state = BULK_CHARGING;
TRACE_ERROR("Charging State machine entered Unknown State. Restarting with Bulk Charging \n");
}
}
}
