mirror of https://github.com/ArduPilot/ardupilot
HAL_SITL: handle periph voltage/current and improve timing
This commit is contained in:
parent
2380849b4d
commit
65ced84a0e
|
@ -614,14 +614,15 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
|
|||
return true;
|
||||
}
|
||||
|
||||
while (duration_us > 0) {
|
||||
const uint32_t start_us = AP_HAL::micros();
|
||||
do {
|
||||
uint16_t wait_us = MIN(100, duration_us);
|
||||
// Timeout conversion
|
||||
auto ts = timespec();
|
||||
ts.tv_sec = 0;
|
||||
ts.tv_nsec = wait_us * 1000UL;
|
||||
|
||||
// Blocking here
|
||||
// check FD for input
|
||||
const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
|
||||
|
||||
if (res < 0) {
|
||||
|
@ -633,8 +634,7 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
|
|||
|
||||
// ensure simulator runs
|
||||
hal.scheduler->delay_microseconds(wait_us);
|
||||
duration_us -= wait_us;
|
||||
}
|
||||
} while (AP_HAL::micros() - start_us < duration_us);
|
||||
|
||||
|
||||
// Handling poll output
|
||||
|
|
|
@ -119,6 +119,7 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
|
|||
struct sitl_input input {};
|
||||
sitl_model->update(input);
|
||||
sim_update();
|
||||
update_voltage_current(input, 0);
|
||||
usleep(100);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -548,44 +548,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
|
|||
_sitl->throttle = throttle;
|
||||
}
|
||||
|
||||
float voltage = 0;
|
||||
_current = 0;
|
||||
|
||||
if (_sitl != nullptr) {
|
||||
if (_sitl->state.battery_voltage <= 0) {
|
||||
if (_vehicle == ArduSub) {
|
||||
voltage = _sitl->batt_voltage;
|
||||
for (uint8_t i=0; i<6; i++) {
|
||||
float pwm = input.servos[i];
|
||||
//printf("i: %d, pwm: %.2f\n", i, pwm);
|
||||
float fraction = fabsf((pwm - 1500) / 500.0f);
|
||||
|
||||
voltage -= fraction * 0.5f;
|
||||
|
||||
float draw = fraction * 15;
|
||||
_current += draw;
|
||||
}
|
||||
} else {
|
||||
// simulate simple battery setup
|
||||
// lose 0.7V at full throttle
|
||||
voltage = _sitl->batt_voltage - 0.7f * throttle;
|
||||
|
||||
// assume 50A at full throttle
|
||||
_current = 50.0f * throttle;
|
||||
}
|
||||
} else {
|
||||
// FDM provides voltage and current
|
||||
voltage = _sitl->state.battery_voltage;
|
||||
_current = _sitl->state.battery_current;
|
||||
}
|
||||
}
|
||||
|
||||
// assume 3DR power brick
|
||||
voltage_pin_voltage = (voltage / 10.1f);
|
||||
current_pin_voltage = _current/17.0f;
|
||||
// fake battery2 as just a 25% gain on the first one
|
||||
voltage2_pin_voltage = voltage_pin_voltage * .25f;
|
||||
current2_pin_voltage = current_pin_voltage * .25f;
|
||||
update_voltage_current(input, throttle);
|
||||
}
|
||||
|
||||
void SITL_State::init(int argc, char * const argv[])
|
||||
|
|
|
@ -19,14 +19,6 @@ class HALSITL::SITL_State : public SITL_State_Common {
|
|||
public:
|
||||
void init(int argc, char * const argv[]);
|
||||
|
||||
enum vehicle_type {
|
||||
ArduCopter,
|
||||
Rover,
|
||||
ArduPlane,
|
||||
ArduSub,
|
||||
Blimp
|
||||
};
|
||||
|
||||
uint16_t pwm_output[SITL_NUM_CHANNELS];
|
||||
uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
|
||||
bool output_ready = false;
|
||||
|
@ -90,7 +82,6 @@ private:
|
|||
void wait_clock(uint64_t wait_time_usec);
|
||||
|
||||
// internal state
|
||||
enum vehicle_type _vehicle;
|
||||
uint8_t _instance;
|
||||
uint16_t _base_port;
|
||||
pid_t _parent_pid;
|
||||
|
@ -102,7 +93,6 @@ private:
|
|||
uint16_t _rcin_port;
|
||||
uint16_t _fg_view_port;
|
||||
uint16_t _irlock_port;
|
||||
float _current;
|
||||
|
||||
bool _synthetic_clock_mode;
|
||||
|
||||
|
|
|
@ -410,5 +410,50 @@ void SITL_State_Common::sim_update(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update voltage and current pins
|
||||
*/
|
||||
void SITL_State_Common::update_voltage_current(struct sitl_input &input, float throttle)
|
||||
{
|
||||
float voltage = 0;
|
||||
float current = 0;
|
||||
|
||||
if (_sitl != nullptr) {
|
||||
if (_sitl->state.battery_voltage <= 0) {
|
||||
if (_vehicle == ArduSub) {
|
||||
voltage = _sitl->batt_voltage;
|
||||
for (uint8_t i=0; i<6; i++) {
|
||||
float pwm = input.servos[i];
|
||||
//printf("i: %d, pwm: %.2f\n", i, pwm);
|
||||
float fraction = fabsf((pwm - 1500) / 500.0f);
|
||||
|
||||
voltage -= fraction * 0.5f;
|
||||
|
||||
float draw = fraction * 15;
|
||||
current += draw;
|
||||
}
|
||||
} else {
|
||||
// simulate simple battery setup
|
||||
// lose 0.7V at full throttle
|
||||
voltage = _sitl->batt_voltage - 0.7f * throttle;
|
||||
|
||||
// assume 50A at full throttle
|
||||
current = 50.0f * throttle;
|
||||
}
|
||||
} else {
|
||||
// FDM provides voltage and current
|
||||
voltage = _sitl->state.battery_voltage;
|
||||
current = _sitl->state.battery_current;
|
||||
}
|
||||
}
|
||||
|
||||
// assume 3DR power brick
|
||||
voltage_pin_voltage = (voltage / 10.1f);
|
||||
current_pin_voltage = current/17.0f;
|
||||
// fake battery2 as just a 25% gain on the first one
|
||||
voltage2_pin_voltage = voltage_pin_voltage * .25f;
|
||||
current2_pin_voltage = current_pin_voltage * .25f;
|
||||
}
|
||||
|
||||
#endif // HAL_BOARD_SITL
|
||||
|
||||
|
|
|
@ -75,6 +75,7 @@ public:
|
|||
void init(int argc, char * const argv[]);
|
||||
|
||||
enum vehicle_type {
|
||||
NONE,
|
||||
ArduCopter,
|
||||
Rover,
|
||||
ArduPlane,
|
||||
|
@ -217,12 +218,16 @@ public:
|
|||
void multicast_state_send(void);
|
||||
|
||||
protected:
|
||||
enum vehicle_type _vehicle;
|
||||
|
||||
void sim_update(void);
|
||||
|
||||
// internal SITL model
|
||||
SITL::Aircraft *sitl_model;
|
||||
|
||||
SITL::SIM *_sitl;
|
||||
|
||||
void update_voltage_current(struct sitl_input &input, float throttle);
|
||||
};
|
||||
|
||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
|
Loading…
Reference in New Issue