Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Only resend the mixer configuration if its changed

This commit is contained in:
Michael du Breuil 2016-01-21 19:48:28 -07:00 committed by Andrew Tridgell
parent 6f59c4ae53
commit bcc939930c
2 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/Plane.h
View File

@ -721,6 +721,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);
void update_load_factor(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();

30
ArduPlane/px4_mixer.cpp
View File

@ -21,6 +21,7 @@
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <modules/px4iofirmware/protocol.h>
#include <GCS_MAVLink/include/mavlink/v1.0/checksum.h>
#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};