Browse Source

snapdragon_rc_pwm: send RC anyway.

Let's always send RC even if nothing arrives on the UART just yet.
sbg
Julian Oes 9 years ago committed by Lorenz Meier
parent
commit
4477566d73
  1. 20
      src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp

20
src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp

@ -127,7 +127,7 @@ void task_main(int argc, char *argv[]) @@ -127,7 +127,7 @@ void task_main(int argc, char *argv[])
// timed out
if (pret == 0) {
continue;
// let's run the loop anyway to send RC
}
if (pret < 0) {
@ -150,16 +150,16 @@ void task_main(int argc, char *argv[]) @@ -150,16 +150,16 @@ void task_main(int argc, char *argv[])
}
}
}
}
// check if we have new rc data, if yes send it to snapdragon
bool rc_updated = false;
orb_check(_rc_sub, &rc_updated);
// check if we have new rc data, if yes send it to snapdragon
bool rc_updated = false;
orb_check(_rc_sub, &rc_updated);
if (rc_updated) {
orb_copy(ORB_ID(input_rc), _rc_sub, &_rc);
// send mavlink message
send_rc_mavlink();
}
if (rc_updated) {
orb_copy(ORB_ID(input_rc), _rc_sub, &_rc);
// send mavlink message
send_rc_mavlink();
}
}
@ -425,4 +425,4 @@ int snapdragon_rc_pwm_main(int argc, char *argv[]) @@ -425,4 +425,4 @@ int snapdragon_rc_pwm_main(int argc, char *argv[])
}
return 0;
}
}

Loading…
Cancel
Save