From 88800b38f8035143c443fbba6bbce368ccc42594 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Nov 2012 00:08:04 +0100 Subject: [PATCH 1/4] HIL testing / cleanup for fixed wing and multirotors --- apps/mavlink/mavlink.c | 5 ----- apps/mavlink/orb_listener.c | 40 ++++++++++--------------------------- 2 files changed, 10 insertions(+), 35 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 9b2cfcbba8..6b4643ec7e 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -143,15 +143,10 @@ set_hil_on_off(bool hil_enabled) /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { - //printf("\n HIL ON \n"); - /* Advertise topics */ pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - printf("\n pub_hil_attitude :%i\n", pub_hil_attitude); - printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos); - mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 683964a0d9..6342ce9793 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -459,29 +459,9 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_controls_s actuators; + struct actuator_outputs_s actuators; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); - - if (gcs_link) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); - } + orb_copy(ORB_ID_VEHICLE_CONTROLS, mavlink_subs.act_0_sub, &actuators); /* Only send in HIL mode */ if (mavlink_hil_enabled) { @@ -494,14 +474,14 @@ l_vehicle_attitude_controls(struct listener *l) /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - actuators.control[0], - actuators.control[1], - actuators.control[2], - actuators.control[3], - 0, - 0, - 0, - 0, + actuators.output[0], + actuators.output[1], + actuators.output[2], + actuators.output[3], + actuators.output[4], + actuators.output[5], + actuators.output[6], + actuators.output[7], mavlink_mode, 0); } From 1b322c7764e62e4dec17fb2e38723e89e398bdfb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Nov 2012 23:50:06 +0100 Subject: [PATCH 2/4] Fixed bug in HIL message handling, operational with actuator outputs now --- apps/mavlink/orb_listener.c | 69 ++++++++++++++++++++++++------------- 1 file changed, 45 insertions(+), 24 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 6342ce9793..7a393185f4 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -419,7 +419,7 @@ l_actuator_outputs(struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - if (gcs_link) + if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, l->arg /* port number */, act_outputs.output[0], @@ -430,6 +430,30 @@ l_actuator_outputs(struct listener *l) act_outputs.output[5], act_outputs.output[6], act_outputs.output[7]); + + /* only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7], + mavlink_mode, + 0); + } + } } void @@ -459,31 +483,28 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_outputs_s actuators; + struct actuator_controls_s actuators; - orb_copy(ORB_ID_VEHICLE_CONTROLS, mavlink_subs.act_0_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - actuators.output[0], - actuators.output[1], - actuators.output[2], - actuators.output[3], - actuators.output[4], - actuators.output[5], - actuators.output[6], - actuators.output[7], - mavlink_mode, - 0); + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators.control[3]); } } From 9221addebdef70e92c34fc7c82dc79ed754f2b6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 Nov 2012 18:15:23 +0100 Subject: [PATCH 3/4] Added HIL/fake PWM out mode to be able to run a mixer against HIL --- apps/drivers/px4fmu/fmu.cpp | 71 ++++++++++++++++++++++++++++++++----- 1 file changed, 63 insertions(+), 8 deletions(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index fff713bb5e..7b1f341b44 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -74,6 +74,7 @@ public: enum Mode { MODE_2PWM, MODE_4PWM, + MODE_HIL_8PWM, MODE_NONE }; PX4FMU(); @@ -269,6 +270,12 @@ PX4FMU::set_mode(Mode mode) _update_rate = 50; /* default output rate */ break; + case MODE_HIL_8PWM: + debug("MODE_HIL_8PWM"); + /* do not actually start a pwm device */ + _update_rate = 50; /* default output rate */ + break; + case MODE_NONE: debug("MODE_NONE"); /* disable servo outputs and set a very low update rate */ @@ -321,7 +328,26 @@ PX4FMU::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + unsigned num_outputs; + + /* select the number of (real or virtual) outputs */ + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_HIL_8PWM: + num_outputs = 8; + break; + + case MODE_NONE: + num_outputs = 0; + break; + } log("starting"); @@ -334,7 +360,11 @@ PX4FMU::task_main() if (update_rate_in_ms < 2) update_rate_in_ms = 2; orb_set_interval(_t_actuators, update_rate_in_ms); - up_pwm_servo_set_rate(_update_rate); + + if (_mode != MODE_HIL_8PWM) { + /* do not attempt to set servos in HIL mode */ + up_pwm_servo_set_rate(_update_rate); + } _current_update_rate = _update_rate; } @@ -367,7 +397,11 @@ PX4FMU::task_main() outputs.output[i] = 1500 + (600 * outputs.output[i]); /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); + if (_mode != MODE_HIL_8PWM) { + /* do not attempt to set servos in HIL mode */ + up_pwm_servo_set(i, outputs.output[i]); + } + } /* and publish for anyone that cares to see */ @@ -383,7 +417,14 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); /* update PWM servo armed status */ - up_pwm_servo_arm(aa.armed); + + /* + * armed = system wants to fly + * locked = there is a low-level safety lock + * in place, such as in hardware-in-the-loop + * simulation setups. + */ + up_pwm_servo_arm(aa.armed && !aa.lockdown); } } @@ -419,7 +460,8 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - debug("ioctl 0x%04x 0x%08x", cmd, arg); + // XXX disable debug output, users got confused + //debug("ioctl 0x%04x 0x%08x", cmd, arg); /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); @@ -432,6 +474,10 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_4PWM: ret = pwm_ioctl(filp, cmd, arg); break; + case MODE_HIL_8PWM: + /* do nothing */ + debug("loading mixer for virtual HIL device"); + break; default: debug("not in a PWM mode"); break; @@ -705,6 +751,7 @@ enum PortMode { PORT_GPIO_AND_SERIAL, PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, + PORT_HIL_PWM }; PortMode g_port_mode; @@ -753,6 +800,11 @@ fmu_new_mode(PortMode new_mode, int update_rate) /* select 2-pin PWM mode */ servo_mode = PX4FMU::MODE_2PWM; break; + + case PORT_HIL_PWM: + /* virtual PWM mode */ + servo_mode = PX4FMU::MODE_HIL_8PWM; + break; } /* adjust GPIO config for serial mode(s) */ @@ -882,11 +934,14 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(argv[i], "mode_pwm_gpio")) { new_mode = PORT_PWM_AND_GPIO; + + } else if (!strcmp(argv[i], "mode_hil_pwm")) { + new_mode = PORT_HIL_PWM; } /* look for the optional pwm update rate for the supported modes */ if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { - if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { + if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO || PORT_HIL_PWM) { if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); } else { @@ -912,7 +967,7 @@ fmu_main(int argc, char *argv[]) /* test, etc. here */ - fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, "FMU: unrecognized command, try:\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial,\n mode_pwm_serial, mode_pwm_gpio, mode_hil_pwm\n"); return -EINVAL; } From bb0c7450c8b019fee03831c3b47119330f3b291f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 Nov 2012 18:43:38 +0100 Subject: [PATCH 4/4] Fixed mixer loading for FMU --- apps/drivers/px4fmu/fmu.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 7b1f341b44..dc9e431e4a 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -477,6 +477,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_HIL_8PWM: /* do nothing */ debug("loading mixer for virtual HIL device"); + ret = 0; break; default: debug("not in a PWM mode");