diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-10 00:11:09 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-10 00:11:09 +0200 |
commit | 3140ba658a322d98c6fe7943c5fb72bb061c46de (patch) | |
tree | 65de11b76548578173146d4b6ab36634bf2e7719 /apps | |
parent | 6ea402efdc769a8b2d1a409ed07890f073ba4867 (diff) | |
download | px4-firmware-3140ba658a322d98c6fe7943c5fb72bb061c46de.tar.gz px4-firmware-3140ba658a322d98c6fe7943c5fb72bb061c46de.tar.bz2 px4-firmware-3140ba658a322d98c6fe7943c5fb72bb061c46de.zip |
Fixed HIL enabling, renamed failsafe to better term "lockdown", made sure HIL is actually locking down system. Pending implementation of lockdown in PWM outputs
Diffstat (limited to 'apps')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 2 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 13 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 15 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls.h | 2 |
4 files changed, 22 insertions, 10 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index af88684bb..dd7b1847b 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -313,7 +313,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* for now only spin if armed and immediately shut down * if in failsafe */ - if (armed.armed && !armed.failsafe) { + if (armed.armed && !armed.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { /* Silently lock down motor speeds to zero */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 51ed95114..2c288e92f 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -220,7 +220,10 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat void publish_armed_status(const struct vehicle_status_s *current_status) { struct actuator_armed_s armed; armed.armed = current_status->flag_system_armed; - armed.failsafe = current_status->rc_signal_lost; + /* lock down actuators if required */ + // XXX FIXME Currently any loss of RC will completely disable all actuators + // needs proper failsafe + armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false; orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -559,13 +562,15 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ } } - /* Switch on HIL if in standby */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { + /* Switch on HIL if in standby and not already in HIL mode */ + if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { /* Enable HIL on request */ current_status->flag_hil_enabled = true; ret = OK; state_machine_publish(status_pub, current_status, mavlink_fd); - printf("[commander] Enabling HIL\n"); + publish_armed_status(current_status); + printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); } /* NEVER actually switch off HIL without reboot */ diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 57c459430..db3470cd6 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -409,6 +409,10 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const s *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } + if (c_status->flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + /* set arming state */ if (actuator->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; @@ -1298,10 +1302,10 @@ void handleMessage(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt/1000; - hil_global_pos.vx = hil_state.vx; - hil_global_pos.vy = hil_state.vy; - hil_global_pos.vz = hil_state.vz; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); @@ -1626,6 +1630,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* send heartbeat */ mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + /* switch HIL mode if required */ + set_hil_on_off(v_status.flag_hil_enabled); + /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f, diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h index e6d7c8670..8d4972bae 100644 --- a/apps/uORB/topics/actuator_controls.h +++ b/apps/uORB/topics/actuator_controls.h @@ -68,7 +68,7 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { bool armed; /**< Set to true if system is armed */ - bool failsafe; /**< Set to true if no valid control input is available */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; ORB_DECLARE(actuator_armed); |