Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 14:17:20 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

merge hil into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-08 19:17:03 +01:00
parent aaa13267b9 bb0c7450c8
commit 8768b7ddbf
3 changed files with 89 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4fmu/fmu.cpp
+62 -6
View File
@@ -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);
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,9 +397,13 @@ PX4FMU::task_main()
outputs.output[i] = 1500 + (600 * outputs.output[i]);
/* output to the servo */
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 */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
}
@@ -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;
}
apps/mavlink/mavlink.c
-5
View File
@@ -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 */
apps/mavlink/orb_listener.c
+25 -23
View File
@@ -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