From bc3b66043f57adebdef3980f3a113d2d5362416a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Dec 2012 11:34:51 +0100 Subject: [PATCH] Cleaned up HIL on FMU / IO combo --- apps/commander/commander.c | 16 ++++++++++++++-- apps/drivers/hil/hil.cpp | 1 - apps/drivers/px4io/px4io.cpp | 7 +++---- apps/mavlink/orb_listener.c | 19 ++----------------- apps/sensors/sensors.cpp | 4 ++-- 5 files changed, 21 insertions(+), 26 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 43219c9f96..f61fd053cd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1268,6 +1268,8 @@ int commander_thread_main(int argc, char *argv[]) int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + sensors.battery_voltage_v = 0.0f; + sensors.battery_voltage_valid = false; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1305,7 +1307,13 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + orb_check(sensor_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } else { + sensors.battery_voltage_valid = false; + } orb_check(cmd_sub, &new_data); if (new_data) { @@ -1434,7 +1442,11 @@ int commander_thread_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ - current_status.voltage_battery = battery_voltage; + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + } else { + current_status.voltage_battery = 0.0f; + } /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42f..cb4a48fab2 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -382,7 +382,6 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 1fb65413ab..6953049264 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -410,7 +410,6 @@ PX4IO::task_main() orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); - /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) { /* last resort: catch NaN, INF and out-of-band errors */ @@ -426,6 +425,9 @@ PX4IO::task_main() } } + /* publish actuator outputs */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); + /* and flag for update */ _send_needed = true; } @@ -570,9 +572,6 @@ PX4IO::io_send() for (unsigned i = 0; i < _max_actuators; i++) cmd.servo_command[i] = _outputs.output[i]; - /* publish as we send */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - // XXX relays /* armed and not locked down -> arming ok */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index b0aa401fc2..e763e642a1 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -506,28 +506,13 @@ l_actuator_outputs(struct listener *l) mavlink_mode, 0); } else { - float rudder, throttle; - - /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ - - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); - } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); - } mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, + 0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/, + (act_outputs.output[2] - 900.0f) / 1200.0f, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 94dbd0b21e..bcc383330c 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1001,9 +1001,9 @@ Sensors::ppm_poll() // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS /* roll input - mixed roll and pitch channels */ - manual_control.roll = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled); + manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled); /* pitch input - mixed roll and pitch channels */ - manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); + manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); /* yaw input - stick right is positive and positive rotation */ manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; /* throttle input */