diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 26dc2c3a3d..6943e0628f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -720,6 +720,11 @@ private: // support for quadcopter-plane QuadPlane quadplane{ahrs}; + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + // the crc of the last created PX4Mixer + int32_t last_mixer_crc = -1; +#endif // CONFIG_HAL_BOARD void demo_servos(uint8_t i); void adjust_nav_pitch_throttle(void); @@ -864,7 +869,7 @@ private: void update_fbwb_speed_height(void); void setup_turn_angle(void); bool print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...); - bool create_mixer(char *buf, uint16_t buf_size, const char *filename); + uint16_t create_mixer(char *buf, uint16_t buf_size, const char *filename); bool setup_failsafe_mixing(void); void set_control_channels(void); void init_rc_in(); diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp index b821e480bf..051d2bd655 100644 --- a/ArduPlane/px4_mixer.cpp +++ b/ArduPlane/px4_mixer.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #define PX4_LIM_RC_MIN 900 #define PX4_LIM_RC_MAX 2100 @@ -44,9 +45,9 @@ bool Plane::print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...) } /* - create a PX4 mixer buffer given the current fixed wing parameters + create a PX4 mixer buffer given the current fixed wing parameters, returns the size of the buffer used */ -bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) +uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) { char *buf0 = buf; uint16_t buf_size0 = buf_size; @@ -125,7 +126,7 @@ bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) } else { // a empty output if (!print_buffer(buf, buf_size, "Z:\n")) { - return false; + return 0; } continue; } @@ -169,7 +170,7 @@ bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) in_scale_high, 0, -limit, limit)) { - return false; + return 0; } } else { const RC_Channel *chan1 = RC_Channel::rc_channel(c1); @@ -184,7 +185,7 @@ bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) // of the PX4IO input processing if (!print_buffer(buf, buf_size, "M: 2\n") || !print_buffer(buf, buf_size, "O: %d %d 0 %d %d\n", mix, mix, (int)-scale_max1, (int)scale_max1)) { - return false; + return 0; } int32_t in_scale_low = pwm_scale*(chan1_trim - pwm_min); int32_t in_scale_high = pwm_scale*(pwm_max - chan1_trim); @@ -192,7 +193,7 @@ bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1, in_scale_low, in_scale_high, offset, (int)-scale_max2, (int)scale_max2)) { - return false; + return 0; } in_scale_low = pwm_scale*(chan2_trim - pwm_min); in_scale_high = pwm_scale*(pwm_max - chan2_trim); @@ -201,13 +202,13 @@ bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c2, in_scale_low, in_scale_high, offset, (int)-scale_max2, (int)scale_max2)) { - return false; + return 0; } } else { if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c2, -in_scale_low, -in_scale_high, -offset, (int)-scale_max2, (int)scale_max2)) { - return false; + return 0; } } } @@ -221,7 +222,7 @@ bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) write(mix_fd, buf0, buf_size0 - buf_size); close(mix_fd); } - return true; + return buf_size0 - buf_size; } @@ -240,12 +241,21 @@ bool Plane::setup_failsafe_mixing(void) return false; } - if (!create_mixer(buf, buf_size, mixer_filename)) { + uint16_t fileSize = create_mixer(buf, buf_size, mixer_filename); + if (!fileSize) { hal.console->printf("Unable to create mixer\n"); free(buf); return false; } + uint16_t new_crc = crc_calculate((uint8_t *)buf, fileSize); + + if ((int32_t)new_crc == last_mixer_crc) { + return true; + } else { + last_mixer_crc = new_crc; + } + enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state(); struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};