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

renamed some command variables to align with Arduplane 

reworked the arming code and moved the DCM gains out.
updated climb_rate to include sonar data.
This commit is contained in:
Jason Short 2011-11-04 21:41:51 -07:00
parent 3aee902be1
commit c8304114a3
15 changed files with 239 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -39,7 +39,7 @@
CH7_SAVE_WP
*/
#define ACCEL_ALT_HOLD 1 // disabled by default, work in progress
#define ACCEL_ALT_HOLD 0 // disabled by default, work in progress
// See the config.h and defines.h files for how to set this up!
//

45
ArduCopter/ArduCopter.pde
View File

@ -303,7 +303,6 @@ static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static byte wp_control; // used to control - navgation or loiter
static byte command_must_index; // current command memory location
@ -335,9 +334,6 @@ static int airspeed; // m/s * 100
// Location Errors
// ---------------
static long altitude_error; // meters * 100 we are off in altitude
static long old_altitude;
static int old_rate;
static long yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging
@ -361,10 +357,20 @@ static int ground_temperature;
// Altitude Sensor variables
// ----------------------
static int sonar_alt;
static int baro_alt;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int altitude_rate;
static long altitude_error; // meters * 100 we are off in altitude
static int climb_rate; // m/s * 100
static int sonar_alt;
static int old_sonar_alt;
static int sonar_rate;
static int baro_alt;
static int old_baro_alt;
static int baro_rate;
// flight mode specific
// --------------------
@ -1206,12 +1212,21 @@ static void update_altitude()
return;
#else
// calc the vertical accel rate
int temp_alt = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
baro_rate = (temp_alt - old_baro_alt) * 10;
old_baro_alt = temp_alt;
if(g.sonar_enabled){
// filter out offset
float scale;
// read barometer
baro_alt = (baro_alt + read_barometer()) >> 1;
baro_alt = (baro_alt + read_barometer()) >> 1;
// calc rate of change for Sonar
sonar_rate = (sonar_alt - old_sonar_alt) * 10;
old_sonar_alt = sonar_alt;
if(baro_alt < 1000){
@ -1224,21 +1239,23 @@ static void update_altitude()
scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
}else{
current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate;
}
}else{
// No Sonar Case
baro_alt = read_barometer();
// no sonar altitude
current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate;
}
// calc the vertical accel rate
int temp_rate = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
altitude_rate = (temp_rate - old_rate) * 10;
old_rate = temp_rate;
#endif
}

2
ArduCopter/Attitude.pde
View File

@ -91,7 +91,7 @@ get_nav_throttle(long z_error)
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
rate_error = rate_error - altitude_rate;
rate_error = rate_error - climb_rate;
// limit the rate
rate_error = constrain(rate_error, -80, 140);

38
ArduCopter/GCS_Mavlink.pde
View File

@ -138,23 +138,23 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
#if FRAME_CONFIG == HELI_FRAME
#if FRAME_CONFIG == HELI_FRAME
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
0,
0,
0,
0,
rssi);
#else
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
@ -166,7 +166,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
rssi);
#endif
}
@ -273,7 +273,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
g.command_index);
}
static void NOINLINE send_statustext(mavlink_channel_t chan)
@ -579,7 +579,7 @@ GCS_MAVLINK::update(void)
uint32_t tnow = millis();
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total &&
waypoint_request_i <= (unsigned)g.command_total &&
tnow > waypoint_timelast_request + 500) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
@ -947,7 +947,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_count_send(
chan,msg->sysid,
msg->compid,
g.waypoint_total + 1); // + home
g.command_total + 1); // + home
waypoint_timelast_send = millis();
waypoint_sending = true;
@ -974,7 +974,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
// send waypoint
tell_command = get_command_with_index(packet.seq);
tell_command = get_cmd_with_index(packet.seq);
// set frame of waypoint
uint8_t frame;
@ -990,7 +990,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == (uint16_t)g.waypoint_index)
if (packet.seq == (uint16_t)g.command_index)
current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false)
@ -1115,7 +1115,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// clear all waypoints
uint8_t type = 0; // ok (0), error(1)
g.waypoint_total.set_and_save(0);
g.command_total.set_and_save(0);
// send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++)
@ -1136,7 +1136,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set current command
change_command(packet.seq);
mavlink_msg_waypoint_current_send(chan, g.waypoint_index);
mavlink_msg_waypoint_current_send(chan, g.command_index);
break;
}
@ -1153,7 +1153,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.count > MAX_WAYPOINTS) {
packet.count = MAX_WAYPOINTS;
}
g.waypoint_total.set_and_save(packet.count - 1);
g.command_total.set_and_save(packet.count - 1);
waypoint_timelast_receive = millis();
waypoint_receiving = true;
@ -1308,7 +1308,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_timelast_request = 0;
waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.waypoint_total){
if (waypoint_request_i > (uint16_t)g.command_total){
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(
@ -1643,7 +1643,7 @@ void
GCS_MAVLINK::queued_waypoint_send()
{
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) {
waypoint_request_i <= (unsigned)g.command_total) {
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,

2
ArduCopter/Log.pde
View File

@ -762,7 +762,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(g.waypoint_total);
DataFlash.WriteByte(g.command_total);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);

14
ArduCopter/Parameters.h
View File

@ -21,7 +21,7 @@ public:
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 10; // 0 for APM trunk
@ -146,8 +146,8 @@ public:
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_command_total,
k_param_command_index,
k_param_command_must_index,
k_param_waypoint_radius,
k_param_loiter_radius,
@ -193,8 +193,8 @@ public:
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 command_total;
AP_Int8 command_index;
AP_Int8 command_must_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
@ -316,8 +316,8 @@ public:
low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
command_total (0, k_param_command_total, PSTR("WP_TOTAL")),
command_index (0, k_param_command_index, PSTR("WP_INDEX")),
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),

22
ArduCopter/commands.pde
View File

@ -3,7 +3,7 @@
static void init_commands()
{
// zero is home, but we always load the next command (1), in the code.
g.waypoint_index = 0;
g.command_index = 0;
// This are registers for the current may and must commands
// setting to zero will allow them to be written to by new commands
@ -22,13 +22,13 @@ static void clear_command_queue(){
// Getters
// -------
static struct Location get_command_with_index(int i)
static struct Location get_cmd_with_index(int i)
{
struct Location temp;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
if (i > g.command_total) {
// we do not have a valid command to load
// return a WP with a "Blank" id
temp.id = CMD_BLANK;
@ -77,7 +77,7 @@ static struct Location get_command_with_index(int i)
// -------
static void set_command_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
i = constrain(i, 0, g.command_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
@ -100,18 +100,18 @@ static void set_command_with_index(struct Location temp, int i)
static void increment_WP_index()
{
if (g.waypoint_index < g.waypoint_total) {
g.waypoint_index++;
if (g.command_index < g.command_total) {
g.command_index++;
}
SendDebugln(g.waypoint_index,DEC);
SendDebugln(g.command_index,DEC);
}
/*
static void decrement_WP_index()
{
if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1);
if (g.command_index > 0) {
g.command_index.set_and_save(g.command_index - 1);
}
}*/
@ -135,7 +135,7 @@ static Location get_LOITER_home_wp()
next_WP = current_loc;
// read home position
struct Location temp = get_command_with_index(0); // 0 = home
struct Location temp = get_cmd_with_index(0); // 0 = home
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold();
return temp;
@ -149,7 +149,7 @@ It precalculates all the necessary stuff.
static void set_next_WP(struct Location *wp)
{
//SendDebug("MSG <set_next_wp> wp_index: ");
//SendDebugln(g.waypoint_index, DEC);
//SendDebugln(g.command_index, DEC);
// copy the current WP into the OldWP slot
// ---------------------------------------

4
ArduCopter/commands_logic.pde
View File

@ -660,7 +660,7 @@ static void do_jump()
command_may_index = 0;
// set pointer to desired index
g.waypoint_index = next_command.p1 - 1;
g.command_index = next_command.p1 - 1;
} else if (jump == 0){
// we're done, move along
@ -668,7 +668,7 @@ static void do_jump()
} else if (jump == -1) {
// repeat forever
g.waypoint_index = next_command.p1 - 1;
g.command_index = next_command.p1 - 1;
}
}

26
ArduCopter/commands_process.pde
View File

@ -2,22 +2,22 @@
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t index)
static void change_command(uint8_t cmd_index)
{
struct Location temp = get_command_with_index(index);
struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
} else {
command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND;
g.waypoint_index = index - 1;
g.command_index = cmd_index - 1;
update_commands();
}
}
// called by 10 Hz Medium loop
// ---------------------------
// called by 10 Hz loop
// --------------------
static void update_commands(void)
{
// fill command queue with a new command if available
@ -25,10 +25,10 @@ static void update_commands(void)
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if (g.waypoint_index < g.waypoint_total) {
if (g.command_index < g.command_total) {
// only if we have a cmd stored in EEPROM
next_command = get_command_with_index(g.waypoint_index + 1);
next_command = get_cmd_with_index(g.command_index + 1);
//Serial.printf("queue CMD %d\n", next_command.id);
}
}
@ -50,7 +50,7 @@ static void update_commands(void)
// We acted on the queue - let's debug that
// ----------------------------------------
print_wp(&next_command, g.waypoint_index);
print_wp(&next_command, g.command_index);
// invalidate command queue so a new one is loaded
// -----------------------------------------------
@ -88,11 +88,11 @@ process_next_command()
if (next_command.id < MAV_CMD_NAV_LAST ){
// we remember the index of our mission here
command_must_index = g.waypoint_index + 1;
command_must_index = g.command_index + 1;
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command);
Log_Write_Cmd(g.command_index + 1, &next_command);
// Act on the new command
process_must();
@ -106,7 +106,7 @@ process_next_command()
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
// we remember the index of our mission here
command_may_index = g.waypoint_index + 1;
command_may_index = g.command_index + 1;
//SendDebug("MSG <pnc> new may ");
//SendDebugln(next_command.id,DEC);
@ -115,7 +115,7 @@ process_next_command()
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command);
Log_Write_Cmd(g.command_index + 1, &next_command);
process_may();
return true;
@ -128,7 +128,7 @@ process_next_command()
//SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command);
Log_Write_Cmd(g.command_index + 1, &next_command);
process_now();
return true;
}

2
ArduCopter/control_modes.pde
View File

@ -99,7 +99,7 @@ static void read_trim_switch()
// set the next_WP
CH7_wp_index++;
current_loc.id = MAV_CMD_NAV_WAYPOINT;
g.waypoint_total.set_and_save(CH7_wp_index);
g.command_total.set_and_save(CH7_wp_index);
set_command_with_index(current_loc, CH7_wp_index);
}
}

225
ArduCopter/motors.pde
View File

@ -1,9 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define ARM_DELAY 10 // one second
#define DISARM_DELAY 10 // one second
#define LEVEL_DELAY 70 // twelve seconds
#define AUTO_LEVEL_DELAY 90 // twentyfive seconds
// 10 = 1 second
#define ARM_DELAY 30
#define DISARM_DELAY 20
#define LEVEL_DELAY 100
// called at 10hz
@ -11,133 +11,114 @@ static void arm_motors()
{
static int arming_counter;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (g.rc_3.control_in == 0){
// don't allow arming/disarming in anything but manual
if ((g.rc_3.control_in > 0) || (control_mode >= ALT_HOLD) || (arming_counter > LEVEL_DELAY)){
arming_counter = 0;
return;
}
// full right
if (g.rc_4.control_in > 4000) {
// don't allow arming in anything but manual
if (control_mode < ALT_HOLD){
if (arming_counter > AUTO_LEVEL_DELAY){
auto_level_counter = 155;
arming_counter = 0;
}else if (arming_counter == ARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
motor_armed = true;
arming_counter = ARM_DELAY;
#if PIEZO_ARMING == 1
piezo_beep();
piezo_beep();
#endif
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
//dcm.ki_roll_pitch(0.000006);
#endif
// tune down compass
// -----------------
dcm.kp_yaw(0.08);
dcm.ki_yaw(0);
// Remember Orientation
// --------------------
init_simple_bearing();
// init the Z damopener
// --------------------
#if ACCEL_ALT_HOLD == 1
init_z_damper();
#endif
// Reset home position
// ----------------------
if(home_is_set)
init_home();
if(did_ground_start == false){
did_ground_start = true;
startup_ground();
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// temp hack
motor_light = true;
digitalWrite(A_LED_PIN, HIGH);
arming_counter++;
} else{
arming_counter++;
}
}
// full left
}else if (g.rc_4.control_in < -4000) {
//Serial.print(arming_counter, DEC);
if (arming_counter > LEVEL_DELAY){
//Serial.print("init");
imu.init_accel(mavlink_delay);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif
motor_armed = false;
arming_counter = DISARM_DELAY;
compass.save_offsets();
#if PIEZO_ARMING == 1
piezo_beep();
#endif
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
//dcm.kp_roll_pitch(0.12); // higher for fast recovery
//dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif
// tune up compass
// -----------------
dcm.kp_yaw(0.8);
dcm.ki_yaw(0.00004);
// Clear throttle slew
// -------------------
//throttle_slew = 0;
arming_counter++;
}else{
arming_counter++;
}
// centered
}else{
// full right
if (g.rc_4.control_in > 4000) {
if (arming_counter == LEVEL_DELAY){
Serial.printf("\nAL\n");
// begin auto leveling
auto_level_counter = 200;
arming_counter = 0;
}else if (arming_counter == ARM_DELAY){
if(motor_armed == false){
// arm the motors and configure for flight
init_arm_motors();
}
// keep going up
arming_counter++;
} else{
arming_counter++;
}
// full left
}else if (g.rc_4.control_in < -4000) {
if (arming_counter == LEVEL_DELAY){
Serial.printf("\nLEV\n");
// begin manual leveling
imu.init_accel(mavlink_delay);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){
if(motor_armed == true){
// arm the motors and configure for flight
init_disarm_motors();
}
// keep going up
arming_counter++;
}else{
arming_counter++;
}
// Yaw is centered
}else{
arming_counter = 0;
}
}
static void init_arm_motors()
{
Serial.printf("\nARM\n");
#if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
motor_armed = true;
#if PIEZO_ARMING == 1
piezo_beep();
piezo_beep();
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
// Reset home position
// ----------------------
if(home_is_set)
init_home();
if(did_ground_start == false){
did_ground_start = true;
startup_ground();
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// temp hack
motor_light = true;
digitalWrite(A_LED_PIN, HIGH);
}
static void init_disarm_motors()
{
Serial.printf("\nDISARM\n");
#if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif
motor_armed = false;
compass.save_offsets();
#if PIEZO_ARMING == 1
piezo_beep();
#endif
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/

6
ArduCopter/setup.pde
View File

@ -815,12 +815,12 @@ static void report_batt_monitor()
static void report_wp(byte index = 255)
{
if(index == 255){
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_command_with_index(i);
for(byte i = 0; i <= g.command_total; i++){
struct Location temp = get_cmd_with_index(i);
print_wp(&temp, i);
}
}else{
struct Location temp = get_command_with_index(index);
struct Location temp = get_cmd_with_index(index);
print_wp(&temp, index);
}
}

14
ArduCopter/system.pde
View File

@ -289,6 +289,20 @@ static void init_ardupilot()
// ---------------------------
reset_control_switch();
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
dcm.kp_yaw(0.08);
dcm.ki_yaw(0.00004);
#endif
// init the Z damopener
// --------------------
#if ACCEL_ALT_HOLD == 1
init_z_damper();
#endif
startup_ground();
Log_Write_Startup();

30
ArduCopter/test.pde
View File

@ -6,7 +6,7 @@
// are defined below. Order matters to the compiler.
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
//static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
//static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_tri(uint8_t argc, const Menu::arg *argv);
@ -56,7 +56,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"failsafe", test_failsafe},
// {"failsafe", test_failsafe},
// {"stabilize", test_stabilize},
{"gps", test_gps},
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -246,6 +246,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
}
}
/*
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
{
@ -299,6 +300,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
return (0);
#endif
}
*/
/*static int8_t
test_stabilize(uint8_t argc, const Menu::arg *argv)
@ -429,7 +431,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
while(1){
//delay(20);
if (millis() - fast_loopTimer >=5) {
if (millis() - fast_loopTimer >=20) {
// IMU
// ---
@ -447,7 +449,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
update_trig();
}
if(medium_loopCounter == 20){
if(medium_loopCounter == 1){
//read_radio();
medium_loopCounter = 0;
//tuning();
@ -461,10 +463,13 @@ test_imu(uint8_t argc, const Menu::arg *argv)
dcm.kp_roll_pitch(),
(float)g.rc_6.control_in / 2000.0);
*/
Serial.printf_P(PSTR("%ld, %ld, %ld\n"),
Serial.printf_P(PSTR("%ld, %ld, %ld, | %ld, %ld, %ld\n"),
dcm.roll_sensor,
dcm.pitch_sensor,
dcm.yaw_sensor);
dcm.yaw_sensor,
(long)(degrees(omega.x) * 100.0),
(long)(degrees(omega.y) * 100.0),
(long)(degrees(omega.z) * 100.0));
if(g.compass_enabled){
compass.read(); // Read magnetometer
@ -740,7 +745,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
}
Serial.printf_P(PSTR("%d wp\n"), (int)g.waypoint_total);
Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit rad: %d\n"), (int)g.waypoint_radius);
//Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
@ -800,11 +805,12 @@ test_baro(uint8_t argc, const Menu::arg *argv)
delay(100);
baro_alt = read_barometer();
int temp_rate = (barometer._offset_press - barometer.RawPress) << 1;
altitude_rate = (temp_rate - old_rate) * 10;
old_rate = temp_rate;
int temp_alt = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
baro_rate = (temp_alt - old_baro_alt) * 10;
old_baro_alt = temp_alt;
// 1 2 3 4 5 1 2 3 4 5
Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, altitude_rate, barometer.RawTemp, barometer.RawPress, temp_rate);
Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, climb_rate, barometer.RawTemp, barometer.RawPress, temp_alt);
//Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2);
if(Serial.available() > 0){
return (0);
@ -995,7 +1001,7 @@ test_mission(uint8_t argc, const Menu::arg *argv)
}
g.RTL_altitude.set_and_save(300);
g.waypoint_total.set_and_save(4);
g.command_total.set_and_save(4);
g.waypoint_radius.set_and_save(3);
test_wp(NULL, NULL);

26
ArduPlane/commands_process.pde
View File

@ -3,21 +3,23 @@
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
{
struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index - 1);
process_next_command();
}
}
// called by 10 Hz loop
// --------------------
@ -48,10 +50,10 @@ static void process_next_command()
{
// This function makes sure that we always have a current navigation command
// and loads conditional or immediate commands if applicable
struct Location temp;
byte old_index = 0;
// these are Navigation/Must commands
// ---------------------------------
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
@ -71,7 +73,7 @@ static void process_next_command()
nav_command_ID = next_nav_command.id;
non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
non_nav_command_ID = NO_COMMAND;
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nav_command);
}
@ -87,7 +89,7 @@ static void process_next_command()
} else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
non_nav_command_index++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
@ -106,8 +108,8 @@ static void process_next_command()
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nonnav_command);
}
process_non_nav_command();
process_non_nav_command();
}
}
@ -134,7 +136,7 @@ static void process_non_nav_command()
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command();
} else {
} else {
handle_process_do_command();
// flag command ID so a new one is loaded
// -----------------------------------------