mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
Simulator: Fix battery interface
This commit is contained in:
Lorenz Meier
@@ -259,39 +259,39 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
|
||||
|
||||
update_sensors(&imu);
|
||||
|
||||
/* battery */
|
||||
{
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
// battery simulation
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
const float discharge_interval_us = 60 * 1000 * 1000;
|
||||
const double discharge_interval_us = 60 * 1000 * 1000;
|
||||
|
||||
static hrt_abstime batt_sim_start = now;
|
||||
static hrt_abstime batt_sim_start = 0;
|
||||
|
||||
float cellcount = 3.0f;
|
||||
|
||||
float vbatt = 4.2f * cellcount;
|
||||
float ibatt = 20.0f;
|
||||
|
||||
vbatt -= (0.5f * cellcount) * ((now - batt_sim_start) / discharge_interval_us);
|
||||
|
||||
if (vbatt < (cellcount * 3.7f)) {
|
||||
vbatt = cellcount * 3.7f;
|
||||
}
|
||||
|
||||
battery_status_s battery_status;
|
||||
|
||||
// TODO: don't hard-code throttle.
|
||||
const float throttle = 0.5f;
|
||||
_battery.updateBatteryStatus(now, vbatt, ibatt, throttle, &battery_status);
|
||||
|
||||
/* lazily publish the battery voltage */
|
||||
if (_battery_pub != nullptr) {
|
||||
orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status);
|
||||
|
||||
} else {
|
||||
_battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status);
|
||||
}
|
||||
if (batt_sim_start == 0 || batt_sim_start > now) {
|
||||
batt_sim_start = now;
|
||||
}
|
||||
|
||||
unsigned cellcount = _battery.cell_count();
|
||||
|
||||
float vbatt = _battery.full_cell_voltage() ;
|
||||
float ibatt = -1.0f;
|
||||
|
||||
float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
|
||||
|
||||
vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount;
|
||||
|
||||
if (vbatt < (cellcount * _battery.empty_cell_voltage())) {
|
||||
vbatt = cellcount * _battery.empty_cell_voltage();
|
||||
}
|
||||
|
||||
battery_status_s battery_status = {};
|
||||
|
||||
// TODO: don't hard-code throttle.
|
||||
const float throttle = 0.5f;
|
||||
_battery.updateBatteryStatus(now, vbatt, ibatt, throttle, &battery_status);
|
||||
|
||||
// publish the battery voltage
|
||||
int batt_multi;
|
||||
orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &battery_status, &batt_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -319,12 +319,8 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
|
||||
|
||||
// publish message
|
||||
if (publish) {
|
||||
if (_rc_channels_pub == nullptr) {
|
||||
_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
|
||||
}
|
||||
int rc_multi;
|
||||
orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -560,7 +556,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
|
||||
// have a message, handle it
|
||||
handle_message(&msg, publish);
|
||||
|
||||
if (msg.msgid != 0 && (hrt_system_time() - pstart_time > 5000000)) {
|
||||
if (msg.msgid != 0 && (hrt_system_time() - pstart_time > 1000000)) {
|
||||
PX4_INFO("Got initial simuation data, running sim..");
|
||||
no_sim_data = false;
|
||||
}
|
||||
@@ -798,12 +794,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
|
||||
|
||||
gyro.temperature = imu->temperature;
|
||||
|
||||
if (_gyro_pub == nullptr) {
|
||||
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
|
||||
}
|
||||
int gyro_multi;
|
||||
orb_publish_auto(ORB_ID(sensor_gyro), &_gyro_pub, &gyro, &gyro_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
|
||||
/* accelerometer */
|
||||
@@ -819,13 +811,9 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
|
||||
accel.z = imu->zacc;
|
||||
|
||||
accel.temperature = imu->temperature;
|
||||
|
||||
if (_accel_pub == nullptr) {
|
||||
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
|
||||
}
|
||||
|
||||
int accel_multi;
|
||||
orb_publish_auto(ORB_ID(sensor_accel), &_accel_pub, &accel, &accel_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
|
||||
/* magnetometer */
|
||||
@@ -842,13 +830,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
|
||||
|
||||
mag.temperature = imu->temperature;
|
||||
|
||||
if (_mag_pub == nullptr) {
|
||||
/* publish to the first mag topic */
|
||||
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
|
||||
}
|
||||
int mag_multi;
|
||||
orb_publish_auto(ORB_ID(sensor_mag), &_mag_pub, &mag, &mag_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
|
||||
/* baro */
|
||||
@@ -860,12 +843,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
|
||||
baro.altitude = imu->pressure_alt;
|
||||
baro.temperature = imu->temperature;
|
||||
|
||||
if (_baro_pub == nullptr) {
|
||||
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
|
||||
}
|
||||
int baro_multi;
|
||||
orb_publish_auto(ORB_ID(sensor_baro), &_baro_pub, &baro, &baro_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
|
||||
return OK;
|
||||
@@ -875,33 +854,26 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
|
||||
{
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
/* flow */
|
||||
{
|
||||
struct optical_flow_s flow;
|
||||
memset(&flow, 0, sizeof(flow));
|
||||
struct optical_flow_s flow;
|
||||
memset(&flow, 0, sizeof(flow));
|
||||
|
||||
flow.sensor_id = flow_mavlink->sensor_id;
|
||||
flow.timestamp = timestamp;
|
||||
flow.time_since_last_sonar_update = 0;
|
||||
flow.frame_count_since_last_readout = 0; // ?
|
||||
flow.integration_timespan = flow_mavlink->integration_time_us;
|
||||
flow.sensor_id = flow_mavlink->sensor_id;
|
||||
flow.timestamp = timestamp;
|
||||
flow.time_since_last_sonar_update = 0;
|
||||
flow.frame_count_since_last_readout = 0; // ?
|
||||
flow.integration_timespan = flow_mavlink->integration_time_us;
|
||||
|
||||
flow.ground_distance_m = flow_mavlink->distance;
|
||||
flow.gyro_temperature = flow_mavlink->temperature;
|
||||
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
|
||||
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
|
||||
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
|
||||
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
|
||||
flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
|
||||
flow.quality = flow_mavlink->quality;
|
||||
flow.ground_distance_m = flow_mavlink->distance;
|
||||
flow.gyro_temperature = flow_mavlink->temperature;
|
||||
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
|
||||
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
|
||||
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
|
||||
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
|
||||
flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
|
||||
flow.quality = flow_mavlink->quality;
|
||||
|
||||
if (_flow_pub == nullptr) {
|
||||
_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
|
||||
}
|
||||
}
|
||||
int flow_multi;
|
||||
orb_publish_auto(ORB_ID(optical_flow), &_flow_pub, &flow, &flow_multi, ORB_PRIO_HIGH);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user