Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

Fixed HIL enabling, renamed failsafe to better term "lockdown", made sure HIL is actually locking down system. Pending implementation of lockdown in PWM outputs

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-10 00:11:09 +02:00
parent 6ea402efdc
commit 3140ba658a
4 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+1 -1
View File
@@ -313,7 +313,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* for now only spin if armed and immediately shut down /* for now only spin if armed and immediately shut down
* if in failsafe * if in failsafe
*/ */
if (armed.armed && !armed.failsafe) { if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls); ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else { } else {
/* Silently lock down motor speeds to zero */ /* Silently lock down motor speeds to zero */
apps/commander/state_machine_helper.c
+9 -4
View File
@@ -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) { void publish_armed_status(const struct vehicle_status_s *current_status) {
struct actuator_armed_s armed; struct actuator_armed_s armed;
armed.armed = current_status->flag_system_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_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &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 */ /* 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)) { if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
/* Enable HIL on request */ /* Enable HIL on request */
current_status->flag_hil_enabled = true; current_status->flag_hil_enabled = true;
ret = OK; ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd); 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 */ /* NEVER actually switch off HIL without reboot */
apps/mavlink/mavlink.c
+11 -4
View File
@@ -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; *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
} }
if (c_status->flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* set arming state */ /* set arming state */
if (actuator->armed) { if (actuator->armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_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.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon; hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt/1000; hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx; hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy; hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz; hil_global_pos.vz = hil_state.vz / 100.0f;
/* set timestamp and notify processes (broadcast) */ /* set timestamp and notify processes (broadcast) */
hil_global_pos.timestamp = hrt_absolute_time(); hil_global_pos.timestamp = hrt_absolute_time();
@@ -1626,6 +1630,9 @@ int mavlink_thread_main(int argc, char *argv[])
/* send heartbeat */ /* send heartbeat */
mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); 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) */ /* 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, 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, v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f,
apps/uORB/topics/actuator_controls.h
+1 -1
View File
@@ -68,7 +68,7 @@ ORB_DECLARE(actuator_controls_3);
/** global 'actuator output is live' control. */ /** global 'actuator output is live' control. */
struct actuator_armed_s { struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */ 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); ORB_DECLARE(actuator_armed);