Simulator: Fix battery interface

src/modules/simulator/simulator_mavlink.cpp

@@ -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;
+			if (batt_sim_start == 0 || batt_sim_start > now) {
+				batt_sim_start = now;
+			}
 
-				float vbatt = 4.2f * cellcount;
-				float ibatt = 20.0f;
+			unsigned cellcount = _battery.cell_count();
 
-				vbatt -= (0.5f * cellcount) * ((now - batt_sim_start) / discharge_interval_us);
+			float vbatt = _battery.full_cell_voltage() ;
+			float ibatt = -1.0f;
 
-				if (vbatt < (cellcount * 3.7f)) {
-					vbatt = cellcount * 3.7f;
-				}
+			float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
 
-				battery_status_s battery_status;
+			vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us)))  * cellcount;
 
-				// TODO: don't hard-code throttle.
-				const float throttle = 0.5f;
-				_battery.updateBatteryStatus(now, vbatt, ibatt, throttle, &battery_status);
+			if (vbatt < (cellcount * _battery.empty_cell_voltage())) {
+				vbatt = cellcount * _battery.empty_cell_voltage();
+			}
 
-				/* lazily publish the battery voltage */
-				if (_battery_pub != nullptr) {
-					orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status);
+			battery_status_s battery_status = {};
 
-				} else {
-					_battery_pub = orb_advertise(ORB_ID(battery_status), &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));
-
-		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;
-
-		if (_flow_pub == nullptr) {
-			_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
-
-		} else {
-			orb_publish(ORB_ID(optical_flow), _flow_pub, &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.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;
+
+	int flow_multi;
+	orb_publish_auto(ORB_ID(optical_flow), &_flow_pub, &flow, &flow_multi, ORB_PRIO_HIGH);
 
 	return OK;
 }