// convert input in 0 to +1 range to pwm output int16_t AP_Motors::calc_pwm_output_0to1(float input, const RC_Channel& servo) { int16_t ret; input = constrain_float(input, 0.0f, 1.0f); if (servo.get_reverse()) { input = 1.0f-input; } ret = input * (servo.get_radio_max() - servo.get_radio_min()) + servo.get_radio_min(); return constrain_int16(ret, servo.get_radio_min(), servo.get_radio_max()); }
// convert input in -1 to +1 range to pwm output int16_t AP_Motors::calc_pwm_output_1to1(float input, const RC_Channel& servo) { int16_t ret; input = constrain_float(input, -1.0f, 1.0f); if (servo.get_reverse()) { input = -input; } if (input >= 0.0f) { ret = ((input * (servo.get_radio_max() - servo.get_radio_trim())) + servo.get_radio_trim()); } else { ret = ((input * (servo.get_radio_trim() - servo.get_radio_min())) + servo.get_radio_trim()); } return constrain_int16(ret, servo.get_radio_min(), servo.get_radio_max()); }
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) { // Act based on the function assigned to this button switch (get_button(button)->function(shift)) { case JSButton::button_function_t::k_arm_toggle: if (motors.armed()) { init_disarm_motors(); } else { init_arm_motors(true); } break; case JSButton::button_function_t::k_arm: init_arm_motors(true); break; case JSButton::button_function_t::k_disarm: init_disarm_motors(); break; case JSButton::button_function_t::k_mode_manual: set_mode(MANUAL, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_stabilize: set_mode(STABILIZE, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_depth_hold: set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_auto: set_mode(AUTO, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_guided: set_mode(GUIDED, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_circle: set_mode(CIRCLE, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_acro: set_mode(ACRO, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_poshold: set_mode(POSHOLD, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mount_center: camera_mount.set_angle_targets(0, 0, 0); // for some reason the call to set_angle_targets changes the mode to mavlink targeting! camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING); break; case JSButton::button_function_t::k_mount_tilt_up: cam_tilt = 1900; break; case JSButton::button_function_t::k_mount_tilt_down: cam_tilt = 1100; break; case JSButton::button_function_t::k_camera_trigger: break; case JSButton::button_function_t::k_camera_source_toggle: if (!held) { static bool video_toggle = false; video_toggle = !video_toggle; if (video_toggle) { video_switch = 1900; gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2"); } else { video_switch = 1100; gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1"); } } break; case JSButton::button_function_t::k_mount_pan_right: // Not implemented break; case JSButton::button_function_t::k_mount_pan_left: // Not implemented break; case JSButton::button_function_t::k_lights1_cycle: if (!held) { static bool increasing = true; RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; if (increasing) { lights1 = constrain_float(lights1 + step, min, max); } else { lights1 = constrain_float(lights1 - step, min, max); } if (lights1 >= max || lights1 <= min) { increasing = !increasing; } } break; case JSButton::button_function_t::k_lights1_brighter: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights1 = constrain_float(lights1 + step, min, max); } break; case JSButton::button_function_t::k_lights1_dimmer: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights1 = constrain_float(lights1 - step, min, max); } break; case JSButton::button_function_t::k_lights2_cycle: if (!held) { static bool increasing = true; RC_Channel* chan = RC_Channels::rc_channel(9); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; if (increasing) { lights2 = constrain_float(lights2 + step, min, max); } else { lights2 = constrain_float(lights2 - step, min, max); } if (lights2 >= max || lights2 <= min) { increasing = !increasing; } } break; case JSButton::button_function_t::k_lights2_brighter: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(9); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights2 = constrain_float(lights2 + step, min, max); } break; case JSButton::button_function_t::k_lights2_dimmer: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(9); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights2 = constrain_float(lights2 - step, min, max); } break; case JSButton::button_function_t::k_gain_toggle: if (!held) { static bool lowGain = false; lowGain = !lowGain; if (lowGain) { gain = 0.5f; } else { gain = 1.0f; } gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100); } break; case JSButton::button_function_t::k_gain_inc: if (!held) { // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80)); g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0)); g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10)); if (g.numGainSettings == 1) { gain = constrain_float(g.gain_default, g.minGain, g.maxGain); } else { gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain); } gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100); } break; case JSButton::button_function_t::k_gain_dec: if (!held) { // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80)); g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0)); g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10)); if (g.numGainSettings == 1) { gain = constrain_float(g.gain_default, g.minGain, g.maxGain); } else { gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain); } gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100); } break; case JSButton::button_function_t::k_trim_roll_inc: rollTrim = constrain_float(rollTrim+10,-200,200); break; case JSButton::button_function_t::k_trim_roll_dec: rollTrim = constrain_float(rollTrim-10,-200,200); break; case JSButton::button_function_t::k_trim_pitch_inc: pitchTrim = constrain_float(pitchTrim+10,-200,200); break; case JSButton::button_function_t::k_trim_pitch_dec: pitchTrim = constrain_float(pitchTrim-10,-200,200); break; case JSButton::button_function_t::k_input_hold_set: if(!motors.armed()) { break; } if (!held) { zTrim = abs(z_last-500) > 50 ? z_last-500 : 0; xTrim = abs(x_last) > 50 ? x_last : 0; yTrim = abs(y_last) > 50 ? y_last : 0; bool input_hold_engaged_last = input_hold_engaged; input_hold_engaged = zTrim || xTrim || yTrim; if (input_hold_engaged) { gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set"); } else if (input_hold_engaged_last) { gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Disabled"); } } break; case JSButton::button_function_t::k_relay_1_on: relay.on(0); break; case JSButton::button_function_t::k_relay_1_off: relay.off(0); break; case JSButton::button_function_t::k_relay_1_toggle: if (!held) { relay.toggle(0); } break; case JSButton::button_function_t::k_relay_2_on: relay.on(1); break; case JSButton::button_function_t::k_relay_2_off: relay.off(1); break; case JSButton::button_function_t::k_relay_2_toggle: if (!held) { relay.toggle(1); } break; case JSButton::button_function_t::k_relay_3_on: relay.on(2); break; case JSButton::button_function_t::k_relay_3_off: relay.off(2); break; case JSButton::button_function_t::k_relay_3_toggle: if (!held) { relay.toggle(2); } break; case JSButton::button_function_t::k_relay_4_on: relay.on(3); break; case JSButton::button_function_t::k_relay_4_off: relay.off(3); break; case JSButton::button_function_t::k_relay_4_toggle: if (!held) { relay.toggle(3); } break; //////////////////////////////////////////////// // Servo functions // TODO: initialize case JSButton::button_function_t::k_servo_1_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_dec: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_min: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_max: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_dec: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_min: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_max: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_dec: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_min: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_max: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed } break; case JSButton::button_function_t::k_roll_pitch_toggle: if (!held) { roll_pitch_flag = !roll_pitch_flag; } break; case JSButton::button_function_t::k_custom_1: // Not implemented break; case JSButton::button_function_t::k_custom_2: // Not implemented break; case JSButton::button_function_t::k_custom_3: // Not implemented break; case JSButton::button_function_t::k_custom_4: // Not implemented break; case JSButton::button_function_t::k_custom_5: // Not implemented break; case JSButton::button_function_t::k_custom_6: // Not implemented break; } }