fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 00:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged IO feature branch

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-13 11:12:34 +01:00
parent 26faab64e5 03076a72ca
commit 1fc0a6546e
13 changed files with 253 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4io/comms.c
+25 -8
View File
@@ -130,8 +130,10 @@ comms_main(void)
last_report_time = now;
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++)
for (unsigned i = 0; i < system_state.rc_channels; i++) {
report.rc_channel[i] = system_state.rc_channel_data[i];
}
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
@@ -168,26 +170,41 @@ comms_handle_command(const void *buffer, size_t length)
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
system_state.fmu_channel_data[i] = cmd->servo_command[i];
}
/* if the IO is armed and FMU gets disarmed, IO must also disarm */
if(system_state.arm_ok && !cmd->arm_ok) {
/* if IO is armed and FMU gets disarmed, IO must also disarm */
if (system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
system_state.manual_override_ok = cmd->manual_override_ok;
system_state.mixer_fmu_available = true;
system_state.fmu_data_received = true;
/* set PWM update rate if changed (after limiting) */
uint16_t new_servo_rate = cmd->servo_rate;
/* handle changes signalled by FMU */
// if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false;
/* reject faster than 500 Hz updates */
if (new_servo_rate > 500) {
new_servo_rate = 500;
}
/* reject slower than 50 Hz updates */
if (new_servo_rate < 50) {
new_servo_rate = 50;
}
if (system_state.servo_rate != new_servo_rate) {
up_pwm_servo_set_rate(new_servo_rate);
system_state.servo_rate = new_servo_rate;
}
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}