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); 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 namespace
{ {
@ -1158,34 +1167,25 @@ PX4IO::io_handle_alarms(uint16_t alarms)
return 0; return 0;
} }
int void
PX4IO::io_get_status() 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 */ /* only publish if battery has a valid minimum voltage */
if (regs[2] > 3300) { if (vbatt <= 3300) {
battery_status_s battery_status; return;
}
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time(); battery_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */ /* 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 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; battery_status.current_a += _battery_amp_bias;
/* /*
@ -1204,7 +1204,25 @@ PX4IO::io_get_status()
} else { } else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); _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; return ret;
} }