mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-09 17:34:01 -04:00
2.0.26
minor updates, Arming motors now for only Stabilize, Simple, and Acro Modes. V Octo Support git-svn-id: https://arducopter.googlecode.com/svn/trunk@2657 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
56483bc322
commit
d2ce6ebf8d
@ -33,6 +33,7 @@
|
|||||||
/*
|
/*
|
||||||
PLUS_FRAME
|
PLUS_FRAME
|
||||||
X_FRAME
|
X_FRAME
|
||||||
|
V_FRAME
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
@ -45,10 +45,10 @@ version 2.1 of the License, or (at your option) any later version.
|
|||||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||||
|
|
||||||
// Configuration
|
// Configuration
|
||||||
|
#include "defines.h"
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
|
|
||||||
// Local modules
|
// Local modules
|
||||||
#include "defines.h"
|
|
||||||
#include "Parameters.h"
|
#include "Parameters.h"
|
||||||
#include "GCS.h"
|
#include "GCS.h"
|
||||||
#include "HIL.h"
|
#include "HIL.h"
|
||||||
@ -496,13 +496,6 @@ void loop()
|
|||||||
//if (delta_ms_fast_loop > 6)
|
//if (delta_ms_fast_loop > 6)
|
||||||
// Log_Write_Performance();
|
// Log_Write_Performance();
|
||||||
|
|
||||||
/*
|
|
||||||
if(delta_ms_fast_loop > 11){
|
|
||||||
update_timer_light(true);
|
|
||||||
//Serial.println(delta_ms_fast_loop,DEC);
|
|
||||||
}else{
|
|
||||||
update_timer_light(false);
|
|
||||||
}*/
|
|
||||||
|
|
||||||
// Execute the fast loop
|
// Execute the fast loop
|
||||||
// ---------------------
|
// ---------------------
|
||||||
|
@ -9,7 +9,6 @@ void init_camera()
|
|||||||
g.rc_camera_pitch.radio_trim = 1500;
|
g.rc_camera_pitch.radio_trim = 1500;
|
||||||
g.rc_camera_pitch.radio_max = 2000;
|
g.rc_camera_pitch.radio_max = 2000;
|
||||||
|
|
||||||
|
|
||||||
g.rc_camera_roll.set_angle(4500);
|
g.rc_camera_roll.set_angle(4500);
|
||||||
g.rc_camera_roll.radio_min = 1000;
|
g.rc_camera_roll.radio_min = 1000;
|
||||||
g.rc_camera_roll.radio_trim = 1500;
|
g.rc_camera_roll.radio_trim = 1500;
|
||||||
|
@ -44,18 +44,6 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
|||||||
uint8_t nav_mode = MAV_NAV_VECTOR;
|
uint8_t nav_mode = MAV_NAV_VECTOR;
|
||||||
|
|
||||||
switch(control_mode) {
|
switch(control_mode) {
|
||||||
case ACRO:
|
|
||||||
mode = MAV_MODE_MANUAL;
|
|
||||||
break;
|
|
||||||
case STABILIZE:
|
|
||||||
mode = MAV_MODE_GUIDED;
|
|
||||||
break;
|
|
||||||
case SIMPLE:
|
|
||||||
mode = MAV_MODE_TEST1;
|
|
||||||
break;
|
|
||||||
case ALT_HOLD:
|
|
||||||
mode = MAV_MODE_TEST2;
|
|
||||||
break;
|
|
||||||
case LOITER:
|
case LOITER:
|
||||||
mode = MAV_MODE_AUTO;
|
mode = MAV_MODE_AUTO;
|
||||||
nav_mode = MAV_NAV_HOLD;
|
nav_mode = MAV_NAV_HOLD;
|
||||||
@ -68,6 +56,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
|||||||
mode = MAV_MODE_AUTO;
|
mode = MAV_MODE_AUTO;
|
||||||
nav_mode = MAV_NAV_RETURNING;
|
nav_mode = MAV_NAV_RETURNING;
|
||||||
break;
|
break;
|
||||||
|
default:
|
||||||
|
mode = control_mode + 100;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t status = MAV_STATE_ACTIVE;
|
uint8_t status = MAV_STATE_ACTIVE;
|
||||||
|
@ -24,7 +24,7 @@
|
|||||||
|
|
||||||
#define PLUS_FRAME 0
|
#define PLUS_FRAME 0
|
||||||
#define X_FRAME 1
|
#define X_FRAME 1
|
||||||
|
#define V_FRAME 2
|
||||||
|
|
||||||
// LED output
|
// LED output
|
||||||
#define NORMAL_LEDS 0
|
#define NORMAL_LEDS 0
|
||||||
@ -106,8 +106,8 @@
|
|||||||
// ----------------
|
// ----------------
|
||||||
#define STABILIZE 0 // hold level position
|
#define STABILIZE 0 // hold level position
|
||||||
#define ACRO 1 // rate control
|
#define ACRO 1 // rate control
|
||||||
#define ALT_HOLD 2 // AUTO control
|
#define SIMPLE 2 //
|
||||||
#define SIMPLE 3 //
|
#define ALT_HOLD 3 // AUTO control
|
||||||
#define AUTO 4 // AUTO control
|
#define AUTO 4 // AUTO control
|
||||||
#define GCS_AUTO 5 // AUTO control
|
#define GCS_AUTO 5 // AUTO control
|
||||||
#define LOITER 6 // Hold a single location
|
#define LOITER 6 // Hold a single location
|
||||||
|
@ -17,6 +17,8 @@ void arm_motors()
|
|||||||
// full right
|
// full right
|
||||||
if (g.rc_4.control_in > 4000) {
|
if (g.rc_4.control_in > 4000) {
|
||||||
|
|
||||||
|
if (control_mode < ALT_HOLD){
|
||||||
|
|
||||||
if (arming_counter > AUTO_LEVEL_DELAY){
|
if (arming_counter > AUTO_LEVEL_DELAY){
|
||||||
auto_level_counter = 255;
|
auto_level_counter = 255;
|
||||||
arming_counter = 0;
|
arming_counter = 0;
|
||||||
@ -33,6 +35,7 @@ void arm_motors()
|
|||||||
} else{
|
} else{
|
||||||
arming_counter++;
|
arming_counter++;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// full left
|
// full left
|
||||||
}else if (g.rc_4.control_in < -4000) {
|
}else if (g.rc_4.control_in < -4000) {
|
||||||
|
@ -38,7 +38,7 @@ void output_motors_armed()
|
|||||||
motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
|
motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
|
||||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
|
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
|
||||||
|
|
||||||
}else{
|
}if(g.frame_orientation == PLUS_FRAME){
|
||||||
roll_out = (float)g.rc_1.pwm_out * 0.71;
|
roll_out = (float)g.rc_1.pwm_out * 0.71;
|
||||||
pitch_out = (float)g.rc_2.pwm_out * 0.71;
|
pitch_out = (float)g.rc_2.pwm_out * 0.71;
|
||||||
|
|
||||||
@ -57,6 +57,38 @@ void output_motors_armed()
|
|||||||
motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
|
motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
|
||||||
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
|
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
|
||||||
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
|
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
|
||||||
|
|
||||||
|
}else{
|
||||||
|
|
||||||
|
int roll_out2, pitch_out2;
|
||||||
|
int roll_out3, pitch_out3;
|
||||||
|
int roll_out4, pitch_out4;
|
||||||
|
|
||||||
|
roll_out = g.rc_1.pwm_out;
|
||||||
|
pitch_out = g.rc_2.pwm_out;
|
||||||
|
roll_out2 = (float)g.rc_1.pwm_out * 0.833;
|
||||||
|
pitch_out2 = (float)g.rc_2.pwm_out * 0.34;
|
||||||
|
roll_out3 = (float)g.rc_1.pwm_out * 0.666;
|
||||||
|
pitch_out3 = (float)g.rc_2.pwm_out * 0.32;
|
||||||
|
roll_out4 = (float)g.rc_1.pwm_out * 0.5;
|
||||||
|
pitch_out4 = (float)g.rc_2.pwm_out * 0.98;
|
||||||
|
|
||||||
|
//Front side
|
||||||
|
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
|
||||||
|
motor_out[CH_11] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
|
||||||
|
|
||||||
|
//Left side
|
||||||
|
motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT
|
||||||
|
motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK
|
||||||
|
|
||||||
|
//Right side
|
||||||
|
motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK
|
||||||
|
motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT
|
||||||
|
|
||||||
|
//Back side
|
||||||
|
motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT
|
||||||
|
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Yaw
|
// Yaw
|
||||||
|
@ -653,7 +653,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
void clear_offsets()
|
void clear_offsets()
|
||||||
{
|
{
|
||||||
Vector3f _offsets;
|
Vector3f _offsets(0.0,0.0,0.0);
|
||||||
compass.set_offsets(_offsets);
|
compass.set_offsets(_offsets);
|
||||||
compass.save_offsets();
|
compass.save_offsets();
|
||||||
}
|
}
|
||||||
|
@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Create the top-level menu object.
|
// Create the top-level menu object.
|
||||||
MENU(main_menu, "AC 2.0.25 Beta", main_menu_commands);
|
MENU(main_menu, "AC 2.0.26 Beta", main_menu_commands);
|
||||||
|
|
||||||
void init_ardupilot()
|
void init_ardupilot()
|
||||||
{
|
{
|
||||||
|
Loading…
Reference in New Issue
Block a user