dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io: split io_handle_battery() out from io_handle_status() 

ready to add vservo and rssi
This commit is contained in:
Andrew Tridgell 2013-09-12 15:00:34 +10:00 committed by Lorenz Meier
parent 1f19a27e3c
commit f7c3ed3ed6
1 changed files with 52 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/drivers/px4io/px4io.cpp
View File

@ -409,8 +409,17 @@ private:
*/
int io_handle_alarms(uint16_t alarms);
};
/**
* Handle a battery update from IO.
*
* Publish IO battery information if necessary.
*
* @param vbatt vbattery register
* @param status ibatter register
*/
void io_handle_battery(uint16_t vbatt, uint16_t ibatt);
};
namespace
{
@ -1158,34 +1167,25 @@ PX4IO::io_handle_alarms(uint16_t alarms)
return 0;
}
int
PX4IO::io_get_status()
void
PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
{
uint16_t regs[4];
int ret;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
if (ret != OK)
return ret;
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
/* only publish if battery has a valid minimum voltage */
if (regs[2] > 3300) {
battery_status_s battery_status;
if (vbatt <= 3300) {
return;
}
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
battery_status.voltage_v = regs[2] / 1000.0f;
battery_status.voltage_v = vbatt / 1000.0f;
/*
regs[3] contains the raw ADC count, as 12 bit ADC
ibatt contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a += _battery_amp_bias;
/*
@ -1204,7 +1204,25 @@ PX4IO::io_get_status()
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
}
int
PX4IO::io_get_status()
{
uint16_t regs[4];
int ret;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
if (ret != OK)
return ret;
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
io_handle_battery(regs[2], regs[3]);
#endif
return ret;
}