void setup_pwm()
{
// R_FORWARD on P3.4
init_pwm(&TB1CTL);
set_pwm_resolution(&TB1CCR0, PWM_RES);
set_pwm_value(&TB1CCR1, PWM_RES / DIVIDE_BY_TWO);
set_pwm_output(&TB1CCTL1);
P3DIR |= R_FORWARD;
P3SEL0 |= R_FORWARD;
P3SEL1 &= ~R_FORWARD;
start_pwm(&TB1CTL);
// L_FORWARD on P3.6
init_pwm(&TB2CTL);
set_pwm_resolution(&TB2CCR0, PWM_RES);
set_pwm_value(&TB2CCR1, PWM_RES / DIVIDE_BY_TWO);
set_pwm_output(&TB2CCTL1);
P3DIR |= L_FORWARD;
P3SEL0 |= L_FORWARD;
P3SEL1 &= ~L_FORWARD;
start_pwm(&TB2CTL);
// R_REVERSE on P3.5
set_pwm_value(&TB1CCR2, PWM_RES / DIVIDE_BY_TWO);
set_pwm_output(&TB1CCTL2);
P3DIR |= R_REVERSE;
P3SEL0 |= R_REVERSE;
P3SEL1 &= ~R_REVERSE;
// L_REVERSE on P3.7
set_pwm_value(&TB2CCR2, PWM_RES / DIVIDE_BY_TWO);
set_pwm_output(&TB2CCTL2);
P3DIR |= L_REVERSE;
P3SEL0 |= L_REVERSE;
P3SEL1 &= ~L_REVERSE;
}
void handle_message(mavlink_message_t *msg)
{
if (msg->msgid == MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET) {
mavlink_actuator_control_target_t actuator_controls;
mavlink_msg_actuator_control_target_decode(msg, &actuator_controls);
//static unsigned counter = 0;
//if (counter++ % 250 == 0) {
// PX4_INFO("got motor controls %.2f %.2f %.2f %.2f",
// (double)actuator_controls.controls[0],
// (double)actuator_controls.controls[1],
// (double)actuator_controls.controls[2],
// (double)actuator_controls.controls[3]);
//}
set_pwm_output(&actuator_controls);
_last_actuator_controls_received = hrt_absolute_time();
}
}