Browse Source

SITL: use millis/micros/panic functions

mission-4.1.18
Caio Marcelo de Oliveira Filho 9 years ago committed by Randy Mackay
parent
commit
d343bfdc6c
  1. 6
      libraries/SITL/SIM_Gimbal.cpp
  2. 14
      libraries/SITL/SIM_JSBSim.cpp
  3. 2
      libraries/SITL/SITL.cpp

6
libraries/SITL/SIM_Gimbal.cpp

@ -50,7 +50,7 @@ Gimbal::Gimbal(const struct sitl_fdm &_fdm) : @@ -50,7 +50,7 @@ Gimbal::Gimbal(const struct sitl_fdm &_fdm) :
void Gimbal::update(void)
{
// calculate delta time in seconds
uint32_t now_us = hal.scheduler->micros();
uint32_t now_us = AP_HAL::micros();
float delta_t = (now_us - last_update_us) * 1.0e-6f;
last_update_us = now_us;
@ -230,7 +230,7 @@ void Gimbal::send_report(void) @@ -230,7 +230,7 @@ void Gimbal::send_report(void)
if (!seen_heartbeat) {
return;
}
uint32_t now = hal.scheduler->millis();
uint32_t now = AP_HAL::millis();
mavlink_message_t msg;
uint16_t len;
@ -262,7 +262,7 @@ void Gimbal::send_report(void) @@ -262,7 +262,7 @@ void Gimbal::send_report(void)
/*
send a GIMBAL_REPORT message
*/
uint32_t now_us = hal.scheduler->micros();
uint32_t now_us = AP_HAL::micros();
if (now_us - last_report_us > reporting_period_ms*1000UL) {
mavlink_gimbal_report_t gimbal_report;
float delta_time = (now_us - last_report_us) * 1.0e-6f;

14
libraries/SITL/SIM_JSBSim.cpp

@ -80,7 +80,7 @@ bool JSBSim::create_templates(void) @@ -80,7 +80,7 @@ bool JSBSim::create_templates(void)
FILE *f = fopen(jsbsim_script, "w");
if (f == NULL) {
hal.scheduler->panic("Unable to create jsbsim script %s", jsbsim_script);
AP_HAL::panic("Unable to create jsbsim script %s", jsbsim_script);
}
fprintf(f,
"<?xml version=\"1.0\" encoding=\"utf-8\"?>\n"
@ -125,7 +125,7 @@ bool JSBSim::create_templates(void) @@ -125,7 +125,7 @@ bool JSBSim::create_templates(void)
f = fopen(jsbsim_fgout, "w");
if (f == NULL) {
hal.scheduler->panic("Unable to create jsbsim fgout script %s", jsbsim_fgout);
AP_HAL::panic("Unable to create jsbsim fgout script %s", jsbsim_fgout);
}
fprintf(f, "<?xml version=\"1.0\"?>\n"
"<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"udp\" rate=\"1000\"/>\n",
@ -136,7 +136,7 @@ bool JSBSim::create_templates(void) @@ -136,7 +136,7 @@ bool JSBSim::create_templates(void)
asprintf(&jsbsim_reset, "%s/aircraft/%s/reset.xml", autotest_dir, jsbsim_model);
f = fopen(jsbsim_reset, "w");
if (f == NULL) {
hal.scheduler->panic("Unable to create jsbsim reset script %s", jsbsim_reset);
AP_HAL::panic("Unable to create jsbsim reset script %s", jsbsim_reset);
}
float r, p, y;
dcm.to_euler(&r, &p, &y);
@ -177,7 +177,7 @@ bool JSBSim::start_JSBSim(void) @@ -177,7 +177,7 @@ bool JSBSim::start_JSBSim(void)
int p[2];
int devnull = open("/dev/null", O_RDWR);
if (pipe(p) != 0) {
hal.scheduler->panic("Unable to create pipe");
AP_HAL::panic("Unable to create pipe");
}
pid_t child_pid = fork();
if (child_pid == 0) {
@ -219,14 +219,14 @@ bool JSBSim::start_JSBSim(void) @@ -219,14 +219,14 @@ bool JSBSim::start_JSBSim(void)
// read startup to be sure it is running
char c;
if (read(jsbsim_stdout, &c, 1) != 1) {
hal.scheduler->panic("Unable to start JSBSim");
AP_HAL::panic("Unable to start JSBSim");
}
if (!expect("JSBSim Execution beginning")) {
hal.scheduler->panic("Failed to start JSBSim");
AP_HAL::panic("Failed to start JSBSim");
}
if (!open_control_socket()) {
hal.scheduler->panic("Failed to open JSBSim control socket");
AP_HAL::panic("Failed to open JSBSim control socket");
}
fcntl(jsbsim_stdout, F_SETFL, fcntl(jsbsim_stdout, F_GETFL, 0) | O_NONBLOCK);

2
libraries/SITL/SITL.cpp

@ -117,7 +117,7 @@ void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash) @@ -117,7 +117,7 @@ void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash)
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_us : hal.scheduler->micros64(),
time_us : AP_HAL::micros64(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),

Loading…
Cancel
Save