Skip to content

Fied wing updates #304

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 19 commits into
base: master
Choose a base branch
from
Open

Fied wing updates #304

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
ce866ac
New funtions for Fixed wing
Dec 3, 2015
39a986b
Moved Motor code
Jan 1, 2016
f2066ac
Syntax
EmilsPa Jan 1, 2016
e5b6ad3
Syntax
EmilsPa Jan 1, 2016
57a4b9d
Formatting
EmilsPa Jan 1, 2016
e9f73c0
Formatting
EmilsPa Jan 1, 2016
a8b4ff1
Formatting
EmilsPa Jan 1, 2016
912220a
Formatting
EmilsPa Jan 1, 2016
c80cc71
Formatting
EmilsPa Jan 1, 2016
a75f59f
Update fw_nav.c
EmilsPa Jan 2, 2016
f90a1d1
Update mw.c
EmilsPa Jan 2, 2016
e912073
Cleaning up mixerTrustVector
Jan 2, 2016
5991dc9
Update mixer.c
EmilsPa Jan 2, 2016
9905832
Update fw_nav.c
EmilsPa Jan 2, 2016
4b3564a
Update mw.c
EmilsPa Jan 2, 2016
0bbef09
Removed Hex file
Jan 2, 2016
44b2f32
Renaming vector Thrust
Jan 5, 2016
c32a76e
Update mw.c
EmilsPa Jan 5, 2016
9d83e01
Update mw.c
EmilsPa Jan 5, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4,483 changes: 0 additions & 4,483 deletions obj/baseflight.hex
View file
Open in desktop

This file was deleted.

24 changes: 14 additions & 10 deletions src/cli.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,10 +208,12 @@ const clivalue_t valueTable[] = {
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafe_detect_threshold, 100, 2000 },
{ "auto_disarm_board", VAR_UINT8, &mcfg.auto_disarm_board, 0, 60 },
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 },
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 14 },
{ "rssi_aux_max", VAR_UINT16, &mcfg.rssi_aux_max, 0, 1000 },
{ "rssi_adc_channel", VAR_INT8, &mcfg.rssi_adc_channel, 0, 9 },
{ "rssi_adc_max", VAR_INT16, &mcfg.rssi_adc_max, 1, 4095 },
{ "rssi_adc_offset", VAR_INT16, &mcfg.rssi_adc_offset, 0, 4095 },
{ "rc_channel_count", VAR_UINT8, &mcfg.rc_channel_count, 8, 18 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 },
{ "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255},
Expand Down Expand Up @@ -260,16 +262,19 @@ const clivalue_t valueTable[] = {
{ "p_vel", VAR_UINT8, &cfg.P8[PIDVEL], 0, 200 },
{ "i_vel", VAR_UINT8, &cfg.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8, &cfg.D8[PIDVEL], 0, 200 },
{ "fw_vector_thrust", VAR_UINT8, &cfg.fw_vector_thrust, 0, 1},
{ "fw_gps_maxcorr", VAR_INT16, &cfg.fw_gps_maxcorr, -45, 45 },
{ "fw_gps_rudder", VAR_INT16, &cfg.fw_gps_rudder, -45, 45 },
{ "fw_gps_maxclimb", VAR_INT16, &cfg.fw_gps_maxclimb, -45, 45 },
{ "fw_gps_maxdive", VAR_INT16, &cfg.fw_gps_maxdive, -45, 45 },
{ "fw_glide_angle", VAR_UINT8, &cfg.fw_glide_angle, 0, 100 },
{ "fw_climb_throttle", VAR_UINT16, &cfg.fw_climb_throttle, 1000, 2000 },
{ "fw_cruise_throttle", VAR_UINT16, &cfg.fw_cruise_throttle, 1000, 2000 },
{ "fw_idle_throttle", VAR_UINT16, &cfg.fw_idle_throttle, 1000, 2000 },
{ "fw_scaler_throttle", VAR_UINT16, &cfg.fw_scaler_throttle, 0, 15 },
{ "fw_roll_comp", VAR_FLOAT, &cfg.fw_roll_comp, 0, 2 },
{ "fw_rth_alt", VAR_UINT8, &cfg.fw_rth_alt, 0, 200 },
{ "fw_roll_comp", VAR_UINT8, &cfg.fw_roll_comp, 0, 250 },
{ "fw_rth_alt", VAR_UINT8, &cfg.fw_rth_alt, 0, 250 },
{ "fw_cruise_distance", VAR_UINT16, &cfg.fw_cruise_distance, 0, 2000},
};

#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
Expand Down Expand Up @@ -878,7 +883,7 @@ static void cliDump(char *cmdline)
}

// print RC MAPPING
for (i = 0; i < 8; i++)
for (i = 0; i < mcfg.rc_channel_count; i++)
buf[mcfg.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0';
printf("map %s\r\n", buf);
Expand Down Expand Up @@ -990,11 +995,11 @@ static void cliMap(char *cmdline)

len = strlen(cmdline);

if (len == 8) {
if (len == mcfg.rc_channel_count) {
// uppercase it
for (i = 0; i < 8; i++)
for (i = 0; i < mcfg.rc_channel_count; i++)
cmdline[i] = toupper((unsigned char)cmdline[i]);
for (i = 0; i < 8; i++) {
for (i = 0; i < mcfg.rc_channel_count; i++) {
if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
continue;
cliPrint("Must be any order of AETR1234\r\n");
Expand All @@ -1003,7 +1008,7 @@ static void cliMap(char *cmdline)
parseRcChannels(cmdline);
}
cliPrint("Current assignment: ");
for (i = 0; i < 8; i++)
for (i = 0; i < mcfg.rc_channel_count; i++)
out[mcfg.rcmap[i]] = rcChannelLetters[i];
out[i] = '\0';
printf("%s\r\n", out);
Expand Down Expand Up @@ -1042,12 +1047,11 @@ static void cliMixer(char *cmdline)
// Presets for planes. Not functional with current reset
// Really Ugly Hack
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
cfg.dynThrPID = 50;
cfg.dynThrPID = 90;
cfg.rcExpo8 = 0;
cfg.P8[PIDALT] = 30;
cfg.I8[PIDALT] = 20;
cfg.D8[PIDALT] = 45;
cfg.D8[PIDPOSR] = 50; // RTH Alt
cfg.P8[PIDNAVR] = 30;
cfg.I8[PIDNAVR] = 20;
cfg.D8[PIDNAVR] = 45;
Expand Down
12 changes: 8 additions & 4 deletions src/config.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,9 @@

master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
const char rcChannelLetters[] = "AERT123456789LMNOP"; // hack for the char-based channel mapping stuff, 18 channels hard max

static const uint8_t EEPROM_CONF_VERSION = 75;
static const uint8_t EEPROM_CONF_VERSION = 76;
static uint32_t enabledSensors = 0;
static void resetConf(void);
static const uint32_t FLASH_WRITE_ADDR = 0x08000000 + (FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - (CONFIG_SIZE / 1024)));
Expand Down Expand Up @@ -252,7 +252,9 @@ static void resetConf(void)
mcfg.looptime = 3500;
mcfg.emf_avoidance = 0;
mcfg.rssi_aux_channel = 0;
mcfg.rssi_aux_max = 1000;
mcfg.rssi_adc_max = 4095;
mcfg.rc_channel_count = 8;

cfg.pidController = 0;
cfg.P8[ROLL] = 40;
Expand Down Expand Up @@ -308,7 +310,7 @@ static void resetConf(void)
cfg.small_angle = 25;

// Radio
parseRcChannels("AETR1234");
parseRcChannels( "AETR123456789LMNOP" ); //18 channels max
cfg.deadband = 0;
cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 40;
Expand Down Expand Up @@ -353,7 +355,9 @@ static void resetConf(void)
cfg.fw_cruise_throttle = 1500;
cfg.fw_idle_throttle = 1300;
cfg.fw_scaler_throttle = 8;
cfg.fw_roll_comp = 1;
cfg.fw_roll_comp = 100;
cfg.fw_cruise_distance = 500;
cfg.fw_rth_alt = 50;
// control stuff
mcfg.reboot_character = 'R';

Expand Down
4 changes: 2 additions & 2 deletions src/drv_pwm.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ enum {
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors

static pwmPortData_t pwmPorts[MAX_PORTS];
static uint16_t captures[MAX_INPUTS];
static uint16_t captures[MAX_PPM_INPUTS]; // max out the captures array, just in case...
static pwmPortData_t *motors[MAX_MOTORS];
static pwmPortData_t *servos[MAX_SERVOS];
static pwmWriteFuncPtr pwmWritePtr = NULL;
Expand Down Expand Up @@ -315,7 +315,7 @@ static void ppmCallback(uint8_t port, uint16_t capture)
if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe."
chan = 0;
} else {
if (diff > PULSE_MIN && diff < PULSE_MAX && chan < MAX_INPUTS) { // 750 to 2250 ms is our 'valid' channel range
if (diff > PULSE_MIN && diff < PULSE_MAX && chan < MAX_PPM_INPUTS) { // 750 to 2250 ms is our 'valid' channel range
captures[chan] = diff;
failsafeCheck(chan, diff);
}
Expand Down
3 changes: 2 additions & 1 deletion src/drv_pwm.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,8 @@

#define MAX_MOTORS 12
#define MAX_SERVOS 8
#define MAX_INPUTS 8
#define MAX_PWM_INPUTS 8
#define MAX_PPM_INPUTS 16
#define PULSE_1MS (1000) // 1ms pulse width
#define PULSE_MIN (750) // minimum PWM pulse width which is considered valid
#define PULSE_MAX (2250) // maximum PWM pulse width which is considered valid
Expand Down
28 changes: 23 additions & 5 deletions src/fw_nav.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,21 @@
// from gps.c
extern int32_t nav_bearing;
extern int32_t wp_distance;
extern int32_t GPS_WP[2];
extern float GPS_scaleLonDown;

extern PID_PARAM navPID_PARAM;
extern PID_PARAM altPID_PARAM;

#define GPS_UPD_HZ 5 // Set loop time for NavUpdate 5 Hz is enough
#define PITCH_COMP 0.5f // Compensate throttle relative angle of attack
// Candidates for CLI
#define SAFE_NAV_ALT 20 // Safe Altitude during climbouts Wings Level below this Alt. (ex. trees & buildings..)
#define SAFE_NAV_ALT 25 // Safe Altitude during climbouts Wings Level below this Alt. (ex. trees & buildings..)
#define SAFE_DECSCEND_ZONE 50 // Radius around home where descending is OK
// For speedBoost
#define GPS_MINSPEED 500 // 500= ~18km/h
#define I_TERM 0.1f
#define GEO_SKALEFACT 89.832f // Scale to match meters

float navErrorI;
float altErrorI;
Expand All @@ -40,6 +44,19 @@ void fw_nav_reset(void)
}
}

void fw_FlyTo(void) // PatrikE CruiseMode version
{
float wp_lat_diff, wp_lon_diff, scaler;
int32_t holdHeading = GPS_ground_course / 10;
if (holdHeading > 180)
holdHeading -= 360;
scaler = (GEO_SKALEFACT / GPS_scaleLonDown) * cfg.fw_cruise_distance;
wp_lat_diff = cos(holdHeading * 0.0174532925f);
wp_lon_diff = sin(holdHeading * 0.0174532925f) * GPS_scaleLonDown;
GPS_WP[LAT] += wp_lat_diff * scaler;
GPS_WP[LON] += wp_lon_diff * scaler;
}

void fw_nav(void)
{
int16_t GPS_Heading = GPS_ground_course; // Store current bearing
Expand Down Expand Up @@ -130,7 +147,6 @@ void fw_nav(void)
if (abs(navDiff) > 170)
navDiff = 175; // Forced turn.


// PID for Navigating planes.
navDT = (float) (millis() - nav_loopT) / 1000;
nav_loopT = millis();
Expand Down Expand Up @@ -201,16 +217,15 @@ void fw_nav(void)
// Elevator compensation depending on behaviour.
// Prevent stall with Disarmed motor
if (f.MOTORS_STOPPED)
GPS_angle[PITCH] = constrain(GPS_angle[PITCH], 0, cfg.fw_gps_maxdive * 10);
GPS_angle[PITCH] = constrain(GPS_angle[PITCH], -cfg.fw_glide_angle, cfg.fw_gps_maxdive * 10);

// Add elevator compared with rollAngle
if (!f.CLIMBOUT_FW)
GPS_angle[PITCH] -= (abs(angle[ROLL]) * cfg.fw_roll_comp);
GPS_angle[PITCH] -= (abs(angle[ROLL]) * (cfg.fw_roll_comp / 100));

// Throttle compensation depending on behaviour.
// Compensate throttle with pitch Angle
NAV_Thro -= constrain(angle[PITCH] * PITCH_COMP, 0, 450);
NAV_Thro = constrain(NAV_Thro, cfg.fw_idle_throttle, cfg.fw_climb_throttle);

// Force the Plane move forward in headwind with speedBoost
groundSpeed = GPS_speed;
Expand All @@ -221,6 +236,9 @@ void fw_nav(void)

speedBoost = constrain(speedBoost, 0, 500);
NAV_Thro += speedBoost;

// constrain throttle to Max climb.
NAV_Thro = constrain(NAV_Thro, cfg.fw_idle_throttle, cfg.fw_climb_throttle);
}
// End of NavTimer

Expand Down
7 changes: 5 additions & 2 deletions src/gps.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -505,15 +505,15 @@ static void reset_PID(PID *pid)

static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles

// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];

// Currently used WP
static int32_t GPS_WP[2];
int32_t GPS_WP[2];

////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
Expand Down Expand Up @@ -768,6 +768,9 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
GPS_WP[LON] = *lon;

GPS_calc_longitude_scaling(*lat);
if (f.CRUISE_MODE)
fw_FlyTo(); // PatrikE CruiseMode version

GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);

nav_bearing = target_bearing;
Expand Down
12 changes: 8 additions & 4 deletions src/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ int main(void)
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
core.numRCChannels = MAX_PWM_INPUTS;

if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
Expand All @@ -213,13 +213,17 @@ int main(void)
// gpsInit will return if FEATURE_GPS is not enabled.
gpsInit(mcfg.gps_baudrate);
#endif
#ifdef SONAR
// sonar stuff only works with PPM

if (feature(FEATURE_PPM)) {
core.numRCChannels = MAX_PPM_INPUTS;
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
}

core.numAuxChannels = constrain((mcfg.rc_channel_count - 4), 4, 8);

#ifndef CJMCU
if (feature(FEATURE_SOFTSERIAL)) {
Expand Down
28 changes: 16 additions & 12 deletions src/mixer.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ static const motorMixer_t mixerDualcopter[] = {
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
};

static const motorMixer_t mixerTrustVector[] = {
static const motorMixer_t mixerVectorThrust[] = {
{ 1.0f, 0.0f, 0.0f, -0.5f }, // LEFT
{ 1.0f, 0.0f, 0.0f, 0.5f }, // RIGHT
};
Expand All @@ -150,13 +150,13 @@ const mixer_t mixers[] = {
{ 0, 1, NULL }, // * MULTITYPE_GIMBAL
{ 6, 0, mixerY6 }, // MULTITYPE_Y6
{ 6, 0, mixerHex6P }, // MULTITYPE_HEX6
{ 2, 1, mixerTrustVector }, // * MULTITYPE_FLYING_WING
{ 2, 1, mixerVectorThrust }, // * MULTITYPE_FLYING_WING
{ 4, 0, mixerY4 }, // MULTITYPE_Y4
{ 6, 0, mixerHex6X }, // MULTITYPE_HEX6X
{ 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8
{ 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP
{ 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX
{ 1, 1, NULL }, // * MULTITYPE_AIRPLANE
{ 2, 1, mixerVectorThrust }, // * MULTITYPE_AIRPLANE
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
Expand All @@ -166,7 +166,7 @@ const mixer_t mixers[] = {
{ 1, 1, NULL }, // MULTITYPE_SINGLECOPTER
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
{ 1, 1, NULL }, // MULTITYPE_CUSTOM_PLANE
{ 2, 1, mixerVectorThrust }, // MULTITYPE_CUSTOM_PLANE
};

// mixer rule format servo, input, rate, speed, min, max, box
Expand Down Expand Up @@ -545,15 +545,18 @@ void mixTable(void)
}

// motors for non-servo mixes
if (numberMotor > 1)
for (i = 0; i < numberMotor; i++)
if (numberMotor > 1) {
for (i = 0; i < numberMotor; i++) {
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;

if (f.FIXED_WING) {
if (!f.ARMED)
motor[0] = mcfg.mincommand; // Kill throttle when disarmed
else
motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
if (f.FIXED_WING) { // vector_thrust handeling
if (cfg.fw_vector_thrust) {
if (f.PASSTHRU_MODE)
motor[i] = rcCommand[THROTTLE] - rcCommand[YAW] * (i - 0.5f);
} else { // Override mixerVectorThrust
motor[i] = rcCommand[THROTTLE];
}
}
}
}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There has to be a better way to write the above, I don't understand what current version does so surely there's a simpler way.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hmm, I'm actually not understanding why this is necessary?
also, is it vector TRUST or vector THRUST?
So if I understand this new code, the following happens:

  1. for each motor, it writes motor[] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
  2. if it's in fixed wing mode, it replaces motor[] with either rccommand[throttle] or
  3. If it's in fixed wing + "vector trust??", then it replaces with throttle + yaw formula.

Is this correct?

  1. for each motor, it writes motor[] =

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess it's vector THRUST.
The function is.
For Fixed Wing vector THRUST is normally active.
Yaw mixed to motors for "Rudder" function.
In Passthru The axisPids is overwritten by Yaw from RC only.

if cfg.fw_vector_trust = false
Pure Throttle command will be forced to motor[i]

The for loop is used to write motor[i] =


// airplane / servo mixes
Expand Down Expand Up @@ -639,6 +642,7 @@ void mixTable(void)
}
if (!f.ARMED) {
motor[i] = motor_disarmed[i];
f.MOTORS_STOPPED = 1;
}
}
}
Loading