diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index fff713bb5e..dc9e431e4a 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,11 @@ 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"); + ret = 0; + break; default: debug("not in a PWM mode"); break; @@ -705,6 +752,7 @@ enum PortMode { PORT_GPIO_AND_SERIAL, PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, + PORT_HIL_PWM }; PortMode g_port_mode; @@ -753,6 +801,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 +935,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 +968,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; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 9c39b2714f..460faf4462 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 a6ae94495f..50cf9cf3d6 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 system 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 @@ -482,28 +506,6 @@ l_vehicle_attitude_controls(struct listener *l) "ctrl3 ", actuators.control[3]); } - - /* 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.control[0], - actuators.control[1], - actuators.control[2], - actuators.control[3], - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } } void