dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Cleaned up HIL on FMU / IO combo

This commit is contained in:
Lorenz Meier 2012-12-19 11:34:51 +01:00
parent f41e5728fc
commit bc3b66043f
5 changed files with 21 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
apps/commander/commander.c
View File

@ -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

1
apps/drivers/hil/hil.cpp
View File

@ -382,7 +382,6 @@ HIL::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}

7
apps/drivers/px4io/px4io.cpp
View File

@ -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 */

19
apps/mavlink/orb_listener.c
View File

@ -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,

4
apps/sensors/sensors.cpp
View File

@ -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 */