bool AP_Landing_Deepstall::override_servos(void)
{
if (!(stage == DEEPSTALL_STAGE_LAND)) {
return false;
}
SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
if (elevator == nullptr) {
// deepstalls are impossible without these channels, abort the process
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,
"Deepstall: Unable to find the elevator channels");
request_go_around();
return false;
}
// calculate the progress on slewing the elevator
float slew_progress = 1.0f;
if (slew_speed > 0) {
slew_progress = (AP_HAL::millis() - stall_entry_time) / (100.0f * slew_speed);
slew_progress = constrain_float (slew_progress, 0.0f, 1.0f);
}
// mix the elevator to the correct value
elevator->set_output_pwm(linear_interpolate(initial_elevator_pwm, elevator_pwm,
slew_progress, 0.0f, 1.0f));
// use the current airspeed to dictate the travel limits
float airspeed;
landing.ahrs.airspeed_estimate(&airspeed);
// only allow the deepstall steering controller to run below the handoff airspeed
if (slew_progress >= 1.0f || airspeed <= handoff_airspeed) {
// run the steering conntroller
float pid = update_steering();
float travel_limit = constrain_float((handoff_airspeed - airspeed) /
(handoff_airspeed - handoff_lower_limit_airspeed) *
0.5f + 0.5f,
0.5f, 1.0f);
float output = constrain_float(pid, -travel_limit, travel_limit);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, output*4500);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, output*4500);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, output*4500);
} else {
// allow the normal servo control of the channel
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input,
SRV_Channels::get_output_scaled(SRV_Channel::k_aileron));
}
// hand off rudder control to deepstall controlled
return true;
}
/*
* build and send sbus1 frame representing first 16 servo channels
* input arg is pointer to uart
*/
void
AP_SBusOut::update()
{
if (!initialised) {
initialised = true;
init();
}
if (sbus1_uart == nullptr) {
return;
}
// constrain output rate using sbus_frame_interval
static uint32_t last_micros = 0;
uint32_t now = AP_HAL::micros();
if ((now - last_micros) <= sbus_frame_interval) {
return;
}
last_micros = now;
/* construct sbus frame representing channels 1 through 16 (max) */
uint8_t nchan = MIN(NUM_SERVO_CHANNELS, SBUS_CHANNELS);
uint16_t channels[SBUS_CHANNELS] {};
for (unsigned i = 0; i < nchan; ++i) {
SRV_Channel *c = SRV_Channels::srv_channel(i);
if (c == nullptr) {
continue;
}
channels[i] = c->get_output_pwm();
}
uint8_t buffer[SBUS_BSIZE];
sbus_format_frame(channels, nchan, buffer);
#if SBUS_DEBUG
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 1);
#endif
sbus1_uart->write(buffer, sizeof(buffer));
#if SBUS_DEBUG
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 0);
#endif
}
void Sub::handle_jsbutton_release(uint8_t button, bool shift) {
// Act based on the function assigned to this button
switch (get_button(button)->function(shift)) {
case JSButton::button_function_t::k_relay_1_momentary:
relay.off(0);
break;
case JSButton::button_function_t::k_relay_2_momentary:
relay.off(1);
break;
case JSButton::button_function_t::k_relay_3_momentary:
relay.off(2);
break;
case JSButton::button_function_t::k_relay_4_momentary:
relay.off(3);
break;
case JSButton::button_function_t::k_servo_1_min_momentary:
case JSButton::button_function_t::k_servo_1_max_momentary:
{
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_min_momentary:
case JSButton::button_function_t::k_servo_2_max_momentary:
{
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_min_momentary:
case JSButton::button_function_t::k_servo_3_max_momentary:
{
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;
}
}
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;
}
}
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
{
// For attempts to change control mode
control_mode_t next_mode = control_mode;
uint16_t next_mode_switch_pwm = mode_switch_pwm;
// 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_toggle:
if (!held) {
next_mode = (control_mode_t)flight_modes[toggle_mode?1:0].get();
next_mode_switch_pwm = toggle_mode?1300:1100;
toggle_mode = !toggle_mode;
}
break;
case JSButton::button_function_t::k_mode_1:
next_mode = (control_mode_t)flight_modes[0].get();
next_mode_switch_pwm = 1100;
toggle_mode = true;
break;
case JSButton::button_function_t::k_mode_2:
next_mode = (control_mode_t)flight_modes[1].get();
next_mode_switch_pwm = 1300;
toggle_mode = false;
break;
case JSButton::button_function_t::k_mode_3:
next_mode = (control_mode_t)flight_modes[2].get();
next_mode_switch_pwm = 1420;
toggle_mode = false;
break;
case JSButton::button_function_t::k_mode_4:
next_mode = (control_mode_t)flight_modes[3].get();
next_mode_switch_pwm = 1550;
toggle_mode = false;
break;
case JSButton::button_function_t::k_mode_5:
next_mode = (control_mode_t)flight_modes[4].get();
next_mode_switch_pwm = 1690;
toggle_mode = false;
break;
case JSButton::button_function_t::k_mode_6:
next_mode = (control_mode_t)flight_modes[5].get();
next_mode_switch_pwm = 1900;
toggle_mode = false;
break;
case JSButton::button_function_t::k_mount_center:
cam_tilt = g.cam_tilt_center;
break;
case JSButton::button_function_t::k_mount_tilt_up: {
uint8_t i;
// Find the first aux channel configured as mount tilt, if any
if (SRV_Channels::find_channel(SRV_Channel::k_mount_tilt, i)) {
// Get the channel output limits
SRV_Channel *ch = SRV_Channels::srv_channel(i);
uint16_t min = ch->get_output_min();
uint16_t max = ch->get_output_max();
cam_tilt = constrain_int16(cam_tilt-g.cam_tilt_step,min,max);
}
}
break;
case JSButton::button_function_t::k_mount_tilt_down: {
uint8_t i;
// Find the first aux channel configured as mount tilt, if any
if (SRV_Channels::find_channel(SRV_Channel::k_mount_tilt, i)) {
// Get the channel output limits
SRV_Channel *ch = SRV_Channels::srv_channel(i);
uint16_t min = ch->get_output_min();
uint16_t max = ch->get_output_max();
cam_tilt = constrain_int16(cam_tilt+g.cam_tilt_step,min,max);
}
}
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;
if (increasing) {
lights1 = constrain_float(lights1+g.lights_step,1100,1900);
} else {
lights1 = constrain_float(lights1-g.lights_step,1100,1900);
}
if (lights1 >= 1900 || lights1 <= 1100) {
increasing = !increasing;
}
}
break;
case JSButton::button_function_t::k_lights1_brighter:
if (!held) {
lights1 = constrain_float(lights1+g.lights_step,1100,1900);
}
break;
case JSButton::button_function_t::k_lights1_dimmer:
if (!held) {
lights1 = constrain_float(lights1-g.lights_step,1100,1900);
}
break;
case JSButton::button_function_t::k_lights2_cycle:
if (!held) {
static bool increasing = true;
if (increasing) {
lights2 = constrain_float(lights2+g.lights_step,1100,1900);
} else {
lights2 = constrain_float(lights2-g.lights_step,1100,1900);
}
if (lights2 >= 1900 || lights2 <= 1100) {
increasing = !increasing;
}
}
break;
case JSButton::button_function_t::k_lights2_brighter:
if (!held) {
lights2 = constrain_float(lights2+g.lights_step,1100,1900);
}
break;
case JSButton::button_function_t::k_lights2_dimmer:
if (!held) {
lights2 = constrain_float(lights2-g.lights_step,1100,1900);
}
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_fmt(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_fmt(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_fmt(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_toggle:
if (!held) {
zTrim = z_last-500;
xTrim = x_last;
yTrim = y_last;
gcs_send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
}
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;
////////////////////////////////////////////////
// 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;
}
// Update the control mode if needed
if (control_mode != next_mode) {
if (set_mode(next_mode, MODE_REASON_TX_COMMAND)) {
// Notify user
if (ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
// Update CH5 pwm value (For GCS)
mode_switch_pwm = next_mode_switch_pwm;
} else {
// Notify user
if (ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1;
}
}
}
}