mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1719 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
767fcf76bb
commit
2555abcf1d
@ -638,36 +638,6 @@
|
|||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
// Subsystem test and debug.
|
|
||||||
//
|
|
||||||
// DEBUG_SUBSYSTEM DEBUG
|
|
||||||
//
|
|
||||||
// Selects a subsystem debug mode. Default is 0.
|
|
||||||
//
|
|
||||||
// 0 = no debug
|
|
||||||
// 1 = Debug the Radio input
|
|
||||||
// 2 = Radio Setup / Servo output
|
|
||||||
// 4 = Debug the GPS input
|
|
||||||
// 5 = Debug the GPS input - RAW OUTPUT
|
|
||||||
// 6 = Debug the IMU
|
|
||||||
// 7 = Debug the Control Switch
|
|
||||||
// 8 = Debug the Servo DIP switches
|
|
||||||
// 9 = Debug the Relay out
|
|
||||||
// 10 = Debug the Magnetometer
|
|
||||||
// 11 = Debug the ABS pressure sensor
|
|
||||||
// 12 = Debug the stored waypoints
|
|
||||||
// 13 = Debug the Throttle
|
|
||||||
// 14 = Debug the Radio Min Max
|
|
||||||
// 15 = Debug the EEPROM - Hex Dump
|
|
||||||
// 16 = XBee X-CTU Range and RSSI Test
|
|
||||||
// 17 = Debug IMU - raw gyro and accel outputs
|
|
||||||
// 20 = Debug Analog Sensors
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//#define DEBUG_SUBSYSTEM 0
|
|
||||||
//
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// DEBUG_LEVEL DEBUG
|
// DEBUG_LEVEL DEBUG
|
||||||
//
|
//
|
||||||
|
|||||||
52
ArduCopterMega/APM_Config_mavlink_hil.h
Normal file
52
ArduCopterMega/APM_Config_mavlink_hil.h
Normal file
@ -0,0 +1,52 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
|
// Hardware in the loop protocol
|
||||||
|
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
|
||||||
|
|
||||||
|
// HIL_MODE SELECTION
|
||||||
|
//
|
||||||
|
// Mavlink supports
|
||||||
|
// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
|
||||||
|
// 2. HIL_MODE_SENSORS: full sensor simulation
|
||||||
|
#define HIL_MODE HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
// HIL_PORT SELCTION
|
||||||
|
//
|
||||||
|
// PORT 1
|
||||||
|
// If you would like to run telemetry communications for a groundstation
|
||||||
|
// while you are running hardware in the loop it is necessary to set
|
||||||
|
// HIL_PORT to 1. This uses the port that would have been used for the gps
|
||||||
|
// as the hardware in the loop port. You will have to solder
|
||||||
|
// headers onto the gps port connection on the apm
|
||||||
|
// and connect via an ftdi cable.
|
||||||
|
//
|
||||||
|
// The baud rate is set to 115200 in this mode.
|
||||||
|
//
|
||||||
|
// PORT 3
|
||||||
|
// If you don't require telemetry communication with a gcs while running
|
||||||
|
// hardware in the loop you may use the telemetry port as the hardware in
|
||||||
|
// the loop port. Alternatively, use a telemetry/HIL shim like FGShim
|
||||||
|
// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
|
||||||
|
//
|
||||||
|
// The buad rate is controlled by SERIAL3_BAUD in this mode.
|
||||||
|
|
||||||
|
#define HIL_PORT 3
|
||||||
|
|
||||||
|
// You can set your gps protocol here for your actual
|
||||||
|
// hardware and leave it without affecting the hardware
|
||||||
|
// in the loop simulation
|
||||||
|
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
|
||||||
|
|
||||||
|
// Ground control station comms
|
||||||
|
#if HIL_PORT != 3
|
||||||
|
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
|
||||||
|
# define GCS_PORT 3
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Sensors
|
||||||
|
// All sensors are supported in all modes.
|
||||||
|
// The magnetometer is not used in
|
||||||
|
// HIL_MODE_ATTITUDE but you may leave it
|
||||||
|
// enabled if you wish.
|
||||||
|
#define AIRSPEED_SENSOR ENABLED
|
||||||
|
#define MAGNETOMETER ENABLED
|
||||||
14
ArduCopterMega/APM_Config_xplane.h
Normal file
14
ArduCopterMega/APM_Config_xplane.h
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
|
|
||||||
|
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
|
||||||
|
#define HIL_MODE HIL_MODE_ATTITUDE
|
||||||
|
#define HIL_PORT 0
|
||||||
|
|
||||||
|
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
|
||||||
|
#define GCS_PORT 3
|
||||||
|
|
||||||
|
#define AIRSPEED_CRUISE 25
|
||||||
|
|
||||||
|
#define THROTTLE_FAILSAFE ENABLED
|
||||||
|
#define AIRSPEED_SENSOR ENABLED
|
||||||
@ -26,6 +26,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||||||
// Libraries
|
// Libraries
|
||||||
#include <FastSerial.h>
|
#include <FastSerial.h>
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
|
#include <APM_BinComm.h>
|
||||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||||
#include <AP_GPS.h> // ArduPilot GPS library
|
#include <AP_GPS.h> // ArduPilot GPS library
|
||||||
#include <Wire.h> // Arduino I2C lib
|
#include <Wire.h> // Arduino I2C lib
|
||||||
@ -36,7 +37,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||||
#include <AP_IMU.h> // ArduPilot Mega IMU Library
|
#include <AP_IMU.h> // ArduPilot Mega IMU Library
|
||||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||||
#include <PID.h> // ArduPilot Mega RC Library
|
#include <PID.h> // PID library
|
||||||
#include <RC_Channel.h> // RC Channel Library
|
#include <RC_Channel.h> // RC Channel Library
|
||||||
#include <AP_RangeFinder.h> // Range finder library
|
#include <AP_RangeFinder.h> // Range finder library
|
||||||
|
|
||||||
@ -50,6 +51,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||||||
#include "Parameters.h"
|
#include "Parameters.h"
|
||||||
#include "global_data.h"
|
#include "global_data.h"
|
||||||
#include "GCS.h"
|
#include "GCS.h"
|
||||||
|
#include "HIL.h"
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Serial ports
|
// Serial ports
|
||||||
@ -87,6 +89,8 @@ Parameters g;
|
|||||||
// All GPS access should be through this pointer.
|
// All GPS access should be through this pointer.
|
||||||
GPS *g_gps;
|
GPS *g_gps;
|
||||||
|
|
||||||
|
#if HIL_MODE == HIL_MODE_NONE
|
||||||
|
|
||||||
// real sensors
|
// real sensors
|
||||||
AP_ADC_ADS7844 adc;
|
AP_ADC_ADS7844 adc;
|
||||||
APM_BMP085_Class barometer;
|
APM_BMP085_Class barometer;
|
||||||
@ -94,26 +98,54 @@ AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
|||||||
|
|
||||||
// real GPS selection
|
// real GPS selection
|
||||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||||
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
||||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
||||||
AP_GPS_NMEA g_gps_driver(&Serial1);
|
AP_GPS_NMEA g_gps_driver(&Serial1);
|
||||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
||||||
AP_GPS_SIRF g_gps_driver(&Serial1);
|
AP_GPS_SIRF g_gps_driver(&Serial1);
|
||||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
||||||
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
||||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
||||||
AP_GPS_MTK g_gps_driver(&Serial1);
|
AP_GPS_MTK g_gps_driver(&Serial1);
|
||||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
||||||
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
||||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
||||||
AP_GPS_None g_gps_driver(NULL);
|
AP_GPS_None g_gps_driver(NULL);
|
||||||
#else
|
#else
|
||||||
#error Unrecognised GPS_PROTOCOL setting.
|
#error Unrecognised GPS_PROTOCOL setting.
|
||||||
#endif // GPS PROTOCOL
|
#endif // GPS PROTOCOL
|
||||||
|
|
||||||
|
#elif HIL_MODE == HIL_MODE_SENSORS
|
||||||
|
// sensor emulators
|
||||||
|
AP_ADC_HIL adc;
|
||||||
|
APM_BMP085_HIL_Class barometer;
|
||||||
|
AP_Compass_HIL compass;
|
||||||
|
AP_GPS_HIL g_gps_driver(NULL);
|
||||||
|
|
||||||
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); // normal imu
|
#elif HIL_MODE == HIL_MODE_ATTITUDE
|
||||||
AP_DCM dcm(&imu, g_gps);
|
AP_DCM_HIL dcm;
|
||||||
|
AP_GPS_HIL g_gps_driver(NULL);
|
||||||
|
|
||||||
|
#else
|
||||||
|
#error Unrecognised HIL_MODE setting.
|
||||||
|
#endif // HIL MODE
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_DISABLED
|
||||||
|
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
|
||||||
|
GCS_MAVLINK hil;
|
||||||
|
#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
|
||||||
|
HIL_XPLANE hil;
|
||||||
|
#endif // HIL PROTOCOL
|
||||||
|
#endif // HIL_MODE
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
#if HIL_MODE != HIL_MODE_SENSORS
|
||||||
|
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); // normal imu
|
||||||
|
#else
|
||||||
|
AP_IMU_Shim imu; // hil imu
|
||||||
|
#endif
|
||||||
|
AP_DCM dcm(&imu, g_gps); // normal dcm
|
||||||
|
#endif
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// GCS selection
|
// GCS selection
|
||||||
@ -306,7 +338,7 @@ byte command_yaw_dir;
|
|||||||
|
|
||||||
// Waypoints
|
// Waypoints
|
||||||
// ---------
|
// ---------
|
||||||
long GPS_wp_distance; // meters - distance between plane and next waypoint
|
//long GPS_wp_distance; // meters - distance between plane and next waypoint
|
||||||
long wp_distance; // meters - distance between plane and next waypoint
|
long wp_distance; // meters - distance between plane and next waypoint
|
||||||
long wp_totalDistance; // meters - distance between old and next waypoint
|
long wp_totalDistance; // meters - distance between old and next waypoint
|
||||||
//byte wp_total; // # of Commands total including way
|
//byte wp_total; // # of Commands total including way
|
||||||
@ -369,11 +401,12 @@ char display_PID = -1; // Flag used by DebugTerminal to indicate that the ne
|
|||||||
// --------------
|
// --------------
|
||||||
unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
||||||
unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
|
unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
|
||||||
|
uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
|
||||||
int mainLoop_count;
|
int mainLoop_count;
|
||||||
|
|
||||||
unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
|
unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
|
||||||
byte medium_loopCounter; // Counters for branching from main control loop to slower loops
|
byte medium_loopCounter; // Counters for branching from main control loop to slower loops
|
||||||
byte medium_count;
|
uint8_t delta_ms_medium_loop;
|
||||||
|
|
||||||
byte slow_loopCounter;
|
byte slow_loopCounter;
|
||||||
byte superslow_loopCounter;
|
byte superslow_loopCounter;
|
||||||
@ -382,8 +415,6 @@ byte fbw_timer; // for limiting the execution of FBW input
|
|||||||
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||||
unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav
|
unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav
|
||||||
|
|
||||||
uint8_t delta_ms_medium_loop;
|
|
||||||
uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
|
|
||||||
|
|
||||||
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||||
unsigned long dTnav2; // Delta Time in milliseconds for navigation computations
|
unsigned long dTnav2; // Delta Time in milliseconds for navigation computations
|
||||||
@ -424,9 +455,9 @@ void loop()
|
|||||||
if (millis() - perf_mon_timer > 20000) {
|
if (millis() - perf_mon_timer > 20000) {
|
||||||
if (mainLoop_count != 0) {
|
if (mainLoop_count != 0) {
|
||||||
gcs.send_message(MSG_PERF_REPORT);
|
gcs.send_message(MSG_PERF_REPORT);
|
||||||
if (g.log_bitmask & MASK_LOG_PM){
|
if (g.log_bitmask & MASK_LOG_PM)
|
||||||
Log_Write_Performance();
|
Log_Write_Performance();
|
||||||
}
|
|
||||||
resetPerfData();
|
resetPerfData();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -576,7 +607,7 @@ void medium_loop()
|
|||||||
calc_bearing_error();
|
calc_bearing_error();
|
||||||
|
|
||||||
// guess how close we are - fixed observer calc
|
// guess how close we are - fixed observer calc
|
||||||
calc_distance_error();
|
//calc_distance_error();
|
||||||
|
|
||||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
||||||
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
|
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
|
||||||
@ -605,12 +636,16 @@ void slow_loop()
|
|||||||
slow_loopCounter++;
|
slow_loopCounter++;
|
||||||
superslow_loopCounter++;
|
superslow_loopCounter++;
|
||||||
|
|
||||||
if(superslow_loopCounter >= 200){
|
if(superslow_loopCounter >= 200){ // Execute every minute
|
||||||
if (g.log_bitmask & MASK_LOG_CUR)
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
Log_Write_Current();
|
if(g.compass_enabled) {
|
||||||
if(g.compass_enabled){
|
|
||||||
compass.save_offsets();
|
compass.save_offsets();
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if (g.log_bitmask & MASK_LOG_CUR)
|
||||||
|
Log_Write_Current();
|
||||||
|
|
||||||
superslow_loopCounter = 0;
|
superslow_loopCounter = 0;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@ -683,7 +718,7 @@ void update_GPS(void)
|
|||||||
SendDebugln("!! bad loc");
|
SendDebugln("!! bad loc");
|
||||||
ground_start_count = 5;
|
ground_start_count = 5;
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
//Serial.printf("init Home!");
|
//Serial.printf("init Home!");
|
||||||
|
|
||||||
if (g.log_bitmask & MASK_LOG_CMD)
|
if (g.log_bitmask & MASK_LOG_CMD)
|
||||||
@ -787,6 +822,7 @@ void update_current_flight_mode(void)
|
|||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case LOITER:
|
||||||
case STABILIZE:
|
case STABILIZE:
|
||||||
// clear any AP naviagtion values
|
// clear any AP naviagtion values
|
||||||
nav_pitch = 0;
|
nav_pitch = 0;
|
||||||
|
|||||||
@ -217,7 +217,7 @@ void calc_nav_throttle()
|
|||||||
|
|
||||||
g.pid_baro_throttle.kP(t);
|
g.pid_baro_throttle.kP(t);
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
// SONAR
|
// SONAR
|
||||||
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||||
|
|
||||||
@ -245,7 +245,7 @@ void output_manual_yaw()
|
|||||||
{
|
{
|
||||||
if(g.rc_3.control_in == 0){
|
if(g.rc_3.control_in == 0){
|
||||||
clear_yaw_control();
|
clear_yaw_control();
|
||||||
} else {
|
}else{
|
||||||
// Yaw control
|
// Yaw control
|
||||||
if(g.rc_4.control_in == 0){
|
if(g.rc_4.control_in == 0){
|
||||||
//clear_yaw_control();
|
//clear_yaw_control();
|
||||||
|
|||||||
@ -16,7 +16,7 @@ GCS_MAVLINK::init(BetterStream * port)
|
|||||||
if (port == &Serial) { // to split hil vs gcs
|
if (port == &Serial) { // to split hil vs gcs
|
||||||
mavlink_comm_0_port = port;
|
mavlink_comm_0_port = port;
|
||||||
chan = MAVLINK_COMM_0;
|
chan = MAVLINK_COMM_0;
|
||||||
} else {
|
}else{
|
||||||
mavlink_comm_1_port = port;
|
mavlink_comm_1_port = port;
|
||||||
chan = MAVLINK_COMM_1;
|
chan = MAVLINK_COMM_1;
|
||||||
}
|
}
|
||||||
@ -347,7 +347,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
x = tell_command.lat/1.0e7; // local (x), global (longitude)
|
x = tell_command.lat/1.0e7; // local (x), global (longitude)
|
||||||
y = tell_command.lng/1.0e7; // local (y), global (latitude)
|
y = tell_command.lng/1.0e7; // local (y), global (latitude)
|
||||||
z = tell_command.alt/1.0e2; // local (z), global (altitude)
|
z = tell_command.alt/1.0e2; // local (z), global (altitude)
|
||||||
} else {
|
}else{
|
||||||
// command is raw
|
// command is raw
|
||||||
x = tell_command.lat;
|
x = tell_command.lat;
|
||||||
y = tell_command.lng;
|
y = tell_command.lng;
|
||||||
@ -508,7 +508,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
tell_command.lat = packet.x; // in as long no conversion
|
tell_command.lat = packet.x; // in as long no conversion
|
||||||
tell_command.lng = packet.y; // in as long no conversion
|
tell_command.lng = packet.y; // in as long no conversion
|
||||||
tell_command.alt = packet.z; // in as int no conversion
|
tell_command.alt = packet.z; // in as int no conversion
|
||||||
} else {
|
}else{
|
||||||
tell_command.p1 = packet.param1; // in as byte no conversion
|
tell_command.p1 = packet.param1; // in as byte no conversion
|
||||||
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
|
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
|
||||||
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
|
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
|
||||||
|
|||||||
@ -54,7 +54,7 @@ print_log_menu(void)
|
|||||||
Serial.printf_P(PSTR("logs enabled: "));
|
Serial.printf_P(PSTR("logs enabled: "));
|
||||||
if (0 == g.log_bitmask) {
|
if (0 == g.log_bitmask) {
|
||||||
Serial.printf_P(PSTR("none"));
|
Serial.printf_P(PSTR("none"));
|
||||||
} else {
|
}else{
|
||||||
// Macro to make the following code a bit easier on the eye.
|
// Macro to make the following code a bit easier on the eye.
|
||||||
// Pass it the capitalised name of the log option, as defined
|
// Pass it the capitalised name of the log option, as defined
|
||||||
// in defines.h but without the LOG_ prefix. It will check for
|
// in defines.h but without the LOG_ prefix. It will check for
|
||||||
@ -76,7 +76,7 @@ print_log_menu(void)
|
|||||||
|
|
||||||
if (last_log_num == 0) {
|
if (last_log_num == 0) {
|
||||||
Serial.printf_P(PSTR("\nNo logs available for download\n"));
|
Serial.printf_P(PSTR("\nNo logs available for download\n"));
|
||||||
} else {
|
}else{
|
||||||
|
|
||||||
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
|
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
|
||||||
for(int i=1;i<last_log_num+1;i++) {
|
for(int i=1;i<last_log_num+1;i++) {
|
||||||
@ -151,7 +151,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||||||
//
|
//
|
||||||
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
|
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
|
||||||
bits = ~(bits = 0);
|
bits = ~(bits = 0);
|
||||||
} else {
|
}else{
|
||||||
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s
|
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s
|
||||||
TARG(ATTITUDE_FAST);
|
TARG(ATTITUDE_FAST);
|
||||||
TARG(ATTITUDE_MED);
|
TARG(ATTITUDE_MED);
|
||||||
@ -168,7 +168,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
|
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
|
||||||
g.log_bitmask.set_and_save(g.log_bitmask | bits);
|
g.log_bitmask.set_and_save(g.log_bitmask | bits);
|
||||||
} else {
|
}else{
|
||||||
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
|
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
|
||||||
}
|
}
|
||||||
return(0);
|
return(0);
|
||||||
@ -206,7 +206,7 @@ byte get_num_logs(void)
|
|||||||
if(data==LOG_INDEX_MSG){
|
if(data==LOG_INDEX_MSG){
|
||||||
byte num_logs = DataFlash.ReadByte();
|
byte num_logs = DataFlash.ReadByte();
|
||||||
return num_logs;
|
return num_logs;
|
||||||
} else {
|
}else{
|
||||||
log_step=0; // Restart, we have a problem...
|
log_step=0; // Restart, we have a problem...
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@ -248,7 +248,7 @@ void start_new_log(byte num_existing_logs)
|
|||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
DataFlash.FinishWrite();
|
DataFlash.FinishWrite();
|
||||||
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
|
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
|
||||||
} else {
|
}else{
|
||||||
gcs.send_text(SEVERITY_LOW,"<start_new_log> Logs full - logging discontinued");
|
gcs.send_text(SEVERITY_LOW,"<start_new_log> Logs full - logging discontinued");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -285,7 +285,7 @@ void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end
|
|||||||
end_page = find_last_log_page(start_page);
|
end_page = find_last_log_page(start_page);
|
||||||
|
|
||||||
return; // This is the normal exit point
|
return; // This is the normal exit point
|
||||||
} else {
|
}else{
|
||||||
log_step=0; // Restart, we have a problem...
|
log_step=0; // Restart, we have a problem...
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@ -381,6 +381,7 @@ void Log_Write_Startup(byte type)
|
|||||||
|
|
||||||
|
|
||||||
// Write a control tuning packet. Total length : 22 bytes
|
// Write a control tuning packet. Total length : 22 bytes
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
void Log_Write_Control_Tuning()
|
void Log_Write_Control_Tuning()
|
||||||
{
|
{
|
||||||
Vector3f accel = imu.get_accel();
|
Vector3f accel = imu.get_accel();
|
||||||
@ -399,6 +400,7 @@ void Log_Write_Control_Tuning()
|
|||||||
DataFlash.WriteInt((int)(accel.y * 10000));
|
DataFlash.WriteInt((int)(accel.y * 10000));
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Write a navigation tuning packet. Total length : 18 bytes
|
// Write a navigation tuning packet. Total length : 18 bytes
|
||||||
void Log_Write_Nav_Tuning()
|
void Log_Write_Nav_Tuning()
|
||||||
@ -447,6 +449,7 @@ void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Write an raw accel/gyro data packet. Total length : 28 bytes
|
// Write an raw accel/gyro data packet. Total length : 28 bytes
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
void Log_Write_Raw()
|
void Log_Write_Raw()
|
||||||
{
|
{
|
||||||
Vector3f gyro = imu.get_gyro();
|
Vector3f gyro = imu.get_gyro();
|
||||||
@ -466,6 +469,7 @@ void Log_Write_Raw()
|
|||||||
|
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void Log_Write_Current()
|
void Log_Write_Current()
|
||||||
{
|
{
|
||||||
@ -520,9 +524,9 @@ void Log_Read_Nav_Tuning()
|
|||||||
Serial.print((float)DataFlash.ReadInt()/100.0); // Altitude error
|
Serial.print((float)DataFlash.ReadInt()/100.0); // Altitude error
|
||||||
Serial.print(comma);
|
Serial.print(comma);
|
||||||
Serial.print((float)DataFlash.ReadInt()/100.0); // Airspeed
|
Serial.print((float)DataFlash.ReadInt()/100.0); // Airspeed
|
||||||
|
Serial.print(comma);
|
||||||
|
Serial.print((float)DataFlash.ReadInt()/1000.0); // nav_gain_scaler
|
||||||
Serial.println(comma);
|
Serial.println(comma);
|
||||||
//Serial.print((float)DataFlash.ReadInt()/1000.0); // nav_gain_scaler
|
|
||||||
//Serial.println(comma);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read a performance packet
|
// Read a performance packet
|
||||||
@ -708,7 +712,7 @@ void Log_Read(int start_page, int end_page)
|
|||||||
if(data == LOG_GPS_MSG){
|
if(data == LOG_GPS_MSG){
|
||||||
Log_Read_GPS();
|
Log_Read_GPS();
|
||||||
log_step++;
|
log_step++;
|
||||||
} else {
|
}else{
|
||||||
Serial.print("Error Reading Packet: ");
|
Serial.print("Error Reading Packet: ");
|
||||||
Serial.print(packet_count);
|
Serial.print(packet_count);
|
||||||
log_step = 0; // Restart, we have a problem...
|
log_step = 0; // Restart, we have a problem...
|
||||||
|
|||||||
@ -20,52 +20,6 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
|||||||
else return 0; // no error
|
else return 0; // no error
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Send low-priority messages at a maximum rate of xx Hertz
|
|
||||||
*
|
|
||||||
* This function sends messages at a lower rate to not exceed the wireless
|
|
||||||
* bandwidth. It sends one message each time it is called until the buffer is empty.
|
|
||||||
* Call this function with xx Hertz to increase/decrease the bandwidth.
|
|
||||||
*/
|
|
||||||
static void mavlink_queued_send(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
AP_Var *vp;
|
|
||||||
float value;
|
|
||||||
|
|
||||||
// send parameters one by one and prevent cross port comms
|
|
||||||
if (NULL != global_data.parameter_p && global_data.requested_interface == chan) {
|
|
||||||
|
|
||||||
// if the value can't be represented as a float, we will skip it here
|
|
||||||
vp = global_data.parameter_p;
|
|
||||||
value = vp->cast_to_float();
|
|
||||||
if (!isnan(value)) {
|
|
||||||
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
|
|
||||||
|
|
||||||
vp->copy_name(param_name, sizeof(param_name));
|
|
||||||
mavlink_msg_param_value_send(chan,
|
|
||||||
(int8_t*)param_name,
|
|
||||||
value,
|
|
||||||
256,
|
|
||||||
vp->meta_get_handle());
|
|
||||||
}
|
|
||||||
|
|
||||||
// remember the next variable we're going to send
|
|
||||||
global_data.parameter_p = vp->next();
|
|
||||||
}
|
|
||||||
|
|
||||||
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
|
|
||||||
mavdelay++;
|
|
||||||
|
|
||||||
// request waypoints one by one
|
|
||||||
if (global_data.waypoint_receiving && global_data.requested_interface == chan &&
|
|
||||||
global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz
|
|
||||||
{
|
|
||||||
mavlink_msg_waypoint_request_send(chan,
|
|
||||||
global_data.waypoint_dest_sysid,
|
|
||||||
global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
|
|
||||||
mavdelay = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
|
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
|
||||||
{
|
{
|
||||||
@ -137,8 +91,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
|||||||
{
|
{
|
||||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
||||||
mavlink_msg_global_position_int_send(chan,current_loc.lat,
|
mavlink_msg_global_position_int_send(chan,current_loc.lat,
|
||||||
current_loc.lng,current_loc.alt*10,g_gps.ground_speed/1.0e2*rot.a.x,
|
current_loc.lng,current_loc.alt*10,g_gps->ground_speed/1.0e2*rot.a.x,
|
||||||
g_gps.ground_speed/1.0e2*rot.b.x,g_gps.ground_speed/1.0e2*rot.c.x);
|
g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case MSG_LOCAL_LOCATION:
|
case MSG_LOCAL_LOCATION:
|
||||||
@ -146,15 +100,15 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
|||||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
||||||
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth,
|
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth,
|
||||||
ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)),
|
ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)),
|
||||||
(current_loc.alt-home.alt)/1.0e2, g_gps.ground_speed/1.0e2*rot.a.x,
|
(current_loc.alt-home.alt)/1.0e2, g_gps->ground_speed/1.0e2*rot.a.x,
|
||||||
g_gps.ground_speed/1.0e2*rot.b.x,g_gps.ground_speed/1.0e2*rot.c.x);
|
g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case MSG_GPS_RAW:
|
case MSG_GPS_RAW:
|
||||||
{
|
{
|
||||||
mavlink_msg_gps_raw_send(chan,timeStamp,g_gps.status(),
|
mavlink_msg_gps_raw_send(chan,timeStamp,g_gps->status(),
|
||||||
g_gps.latitude/1.0e7,g_gps.longitude/1.0e7,g_gps.altitude/100.0,
|
g_gps->latitude/1.0e7,g_gps->longitude/1.0e7,g_gps->altitude/100.0,
|
||||||
g_gps.hdop,0.0,g_gps.ground_speed/100.0,g_gps.ground_course/100.0);
|
g_gps->hdop,0.0,g_gps->ground_speed/100.0,g_gps->ground_course/100.0);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case MSG_SERVO_OUT:
|
case MSG_SERVO_OUT:
|
||||||
@ -200,7 +154,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
|||||||
}
|
}
|
||||||
case MSG_VFR_HUD:
|
case MSG_VFR_HUD:
|
||||||
{
|
{
|
||||||
mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps.ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
|
mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps->ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
|
||||||
climb_rate, (int)rc[CH_THROTTLE]->servo_out);
|
climb_rate, (int)rc[CH_THROTTLE]->servo_out);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -224,7 +178,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
|||||||
|
|
||||||
case MSG_GPS_STATUS:
|
case MSG_GPS_STATUS:
|
||||||
{
|
{
|
||||||
mavlink_msg_gps_status_send(chan,g_gps.num_sats,NULL,NULL,NULL,NULL,NULL);
|
mavlink_msg_gps_status_send(chan,g_gps->num_sats,NULL,NULL,NULL,NULL,NULL);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -223,7 +223,7 @@ public:
|
|||||||
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
|
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
|
||||||
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
|
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
|
||||||
milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")),
|
milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")),
|
||||||
compass_enabled (DISABLED, k_param_compass_enabled, PSTR("COMPASS_ENABLE")),
|
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
|
||||||
|
|
||||||
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
|
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
|
||||||
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
||||||
|
|||||||
@ -1,80 +1,61 @@
|
|||||||
ArduPilotMega 1.0.0 Commands
|
ArduPilotMega 2.0 Commands
|
||||||
|
|
||||||
Command Structure in bytes
|
Command Structure in bytes
|
||||||
0 0x00 byte Command ID
|
0 0x00 byte Command ID
|
||||||
1 0x01 byte Parameter 1
|
1 0x01 byte Parameter 1
|
||||||
2 0x02 int Parameter 2
|
2 0x02 long Parameter 2
|
||||||
3 0x03 ..
|
3 0x03 ..
|
||||||
4 0x04 long Parameter 3
|
4 0x04 ..
|
||||||
5 0x05 ..
|
5 0x05 ..
|
||||||
6 0x06 ..
|
6 0x06 long Parameter 3
|
||||||
7 0x07 ..
|
7 0x07 ..
|
||||||
8 0x08 long Parameter 4
|
8 0x08 ..
|
||||||
9 0x09 ..
|
9 0x09 ..
|
||||||
10 0x0A ..
|
10 0x0A long Parameter 4
|
||||||
11 0x0B ..
|
11 0x0B ..
|
||||||
|
11 0x0C ..
|
||||||
|
11 0x0D ..
|
||||||
|
|
||||||
|
|
||||||
0x00 Reserved
|
|
||||||
....
|
|
||||||
0x0F Reserved
|
|
||||||
|
|
||||||
***********************************
|
***********************************
|
||||||
Commands 0x10 to 0x2F are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
|
Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
|
||||||
***********************************
|
***********************************
|
||||||
Command ID Name Parameter 1 Altitude Latitude Longitude
|
Command ID Name Parameter 1 Altitude Latitude Longitude
|
||||||
0x10 CMD_WAYPOINT - altitude lat lon
|
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
|
||||||
0x11 CMD_LOITER (indefinitely) - altitude lat lon
|
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) - altitude lat lon
|
||||||
0x12 CMD_LOITER_N_TURNS turns altitude lat lon
|
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
|
||||||
0x13 CMD_LOITER_TIME time (seconds*10) altitude lat lon
|
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
|
||||||
0x14 CMD_RTL - altitude lat lon
|
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
|
||||||
0x15 CMD_LAND - altitude lat lon
|
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
|
||||||
0x16 CMD_TAKEOFF angle altitude - -
|
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
|
||||||
0x17 CMD_ALTITUDE - altitude - -
|
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
|
||||||
0x18 CMD_R_WAYPOINT - altitude rlat rlon
|
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
|
||||||
0x19 CMD_TARGET - altitude lat lon
|
|
||||||
|
|
||||||
|
|
||||||
***********************************
|
***********************************
|
||||||
May Commands - these commands are optional to finish
|
May Commands - these commands are optional to finish
|
||||||
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
|
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
|
||||||
0x20 CMD_DELAY - - time (milliseconds) -
|
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (milliseconds) -
|
||||||
0x21 CMD_CLIMB rate (cm/sec) alt (finish) - -
|
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - -
|
||||||
0x22 CMD_LAND_OPTIONS pitch deg airspeed m/s, throttle %, distance to WP
|
0x72 / 114 MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL)
|
||||||
0x23 CMD_ANGLE angle speed direction (-1,1) rel (1), abs (0)
|
0x23 MAV_CMD_CONDITION_ANGLE angle speed direction (-1,1) rel (1), abs (0)
|
||||||
|
|
||||||
|
|
||||||
***********************************
|
***********************************
|
||||||
Unexecuted commands >= 0x20 are dropped when ready for the next command <= 0x1F so plan/queue commands accordingly!
|
Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
|
||||||
For example if you had a string of 0x21 commands following a 0x10 command that had not finished when the waypoint was
|
For example if you had a string of CMD_MAV_CONDITION commands following a 0x10 command that had not finished when the waypoint was
|
||||||
reached, the unexecuted 0x21 commands would be skipped and the next command <= 0x1F would be loaded
|
reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipped and the next command < MAV_CMD_NAV_LAST would be loaded
|
||||||
***********************************
|
***********************************
|
||||||
Now Commands - these commands are executed once until no more new now commands are available
|
Now Commands - these commands are executed once until no more new now commands are available
|
||||||
0x31 CMD_RESET_INDEX
|
|
||||||
0x32 CMD_GOTO_INDEX index repeat count
|
|
||||||
0x33 CMD_GETVAR_INDEX variable ID
|
|
||||||
0x34 CMD_SENDVAR_INDEX off/on = 0/1
|
|
||||||
0x35 CMD_TELEMETRY off/on = 0/1
|
|
||||||
|
|
||||||
0x40 CMD_THROTTLE_CRUISE speed
|
|
||||||
//0x41 CMD_AIRSPEED_CRUISE speed
|
|
||||||
0x44 CMD_RESET_HOME altitude lat lon
|
|
||||||
|
|
||||||
0x60 CMD_KP_GAIN array index gain value*100,000
|
|
||||||
0x61 CMD_KI_GAIN array index gain value*100,000
|
|
||||||
0x62 CMD_KD_GAIN array index gain value*100,000
|
|
||||||
0x63 CMD_KI_MAX array index gain value*100,000
|
|
||||||
0x64 CMD_KFF_GAIN array index gain value*100,000
|
|
||||||
|
|
||||||
0x70 CMD_RADIO_TRIM array index value
|
|
||||||
0x71 CMD_RADIO_MAX array index value
|
|
||||||
0x72 CMD_RADIO_MIN array index value
|
|
||||||
0x73 CMD_ELEVON_TRIM array index value (index 0 = elevon 1 trim, 1 = elevon 2 trim)
|
|
||||||
0x75 CMD_INDEX index
|
|
||||||
|
|
||||||
0x80 CMD_REPEAT type value delay in sec repeat count
|
|
||||||
0x81 CMD_RELAY (0,1 to change swicth position)
|
|
||||||
0x82 CMD_SERVO number value
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
|
||||||
|
0xB1 / 177 MAV_CMD_DO_JUMP index repeat count
|
||||||
|
0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED TODO - Fill in from protocol
|
||||||
|
0xB3 / 179 MAV_CMD_DO_SET_HOME
|
||||||
|
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER
|
||||||
|
0xB5 / 181 MAV_CMD_DO_SET_RELAY
|
||||||
|
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY
|
||||||
|
0xB7 / 183 MAV_CMD_DO_SET_SERVO
|
||||||
|
0xB8 / 184 MAV_CMD_DO_REPEAT_SERVO
|
||||||
|
|
||||||
|
|||||||
@ -111,13 +111,35 @@ set_current_loc_here()
|
|||||||
set_next_WP(&l);
|
set_next_WP(&l);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void loiter_at_location()
|
||||||
|
{
|
||||||
|
next_WP = current_loc;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_mode_loiter_home(void)
|
||||||
|
{
|
||||||
|
control_mode = LOITER;
|
||||||
|
//crash_timer = 0;
|
||||||
|
|
||||||
|
next_WP = current_loc;
|
||||||
|
// Altitude to hold over home
|
||||||
|
// Set by configuration tool
|
||||||
|
// -------------------------
|
||||||
|
next_WP.alt = read_alt_to_hold();
|
||||||
|
|
||||||
|
// output control mode to the ground station
|
||||||
|
gcs.send_message(MSG_HEARTBEAT);
|
||||||
|
|
||||||
|
if (g.log_bitmask & MASK_LOG_MODE)
|
||||||
|
Log_Write_Mode(control_mode);
|
||||||
|
}
|
||||||
|
|
||||||
//********************************************************************************
|
//********************************************************************************
|
||||||
//This function sets the waypoint and modes for Return to Launch
|
//This function sets the waypoint and modes for Return to Launch
|
||||||
//********************************************************************************
|
//********************************************************************************
|
||||||
|
|
||||||
// add a new command at end of command set to RTL.
|
// add a new command at end of command set to RTL.
|
||||||
void
|
void return_to_launch(void)
|
||||||
return_to_launch(void)
|
|
||||||
{
|
{
|
||||||
//so we know where we are navigating from
|
//so we know where we are navigating from
|
||||||
next_WP = current_loc;
|
next_WP = current_loc;
|
||||||
@ -137,8 +159,7 @@ return_to_launch(void)
|
|||||||
//send_message(SEVERITY_LOW,"Return To Launch");
|
//send_message(SEVERITY_LOW,"Return To Launch");
|
||||||
}
|
}
|
||||||
|
|
||||||
struct
|
struct Location get_LOITER_home_wp()
|
||||||
Location get_LOITER_home_wp()
|
|
||||||
{
|
{
|
||||||
// read home position
|
// read home position
|
||||||
struct Location temp = get_wp_with_index(0);
|
struct Location temp = get_wp_with_index(0);
|
||||||
@ -151,8 +172,7 @@ Location get_LOITER_home_wp()
|
|||||||
This function stores waypoint commands
|
This function stores waypoint commands
|
||||||
It looks to see what the next command type is and finds the last command.
|
It looks to see what the next command type is and finds the last command.
|
||||||
*/
|
*/
|
||||||
void
|
void set_next_WP(struct Location *wp)
|
||||||
set_next_WP(struct Location *wp)
|
|
||||||
{
|
{
|
||||||
//GCS.send_text(SEVERITY_LOW,"load WP");
|
//GCS.send_text(SEVERITY_LOW,"load WP");
|
||||||
SendDebug("MSG <set_next_wp> wp_index: ");
|
SendDebug("MSG <set_next_wp> wp_index: ");
|
||||||
@ -173,8 +193,12 @@ set_next_WP(struct Location *wp)
|
|||||||
|
|
||||||
// used to control FBW and limit the rate of climb
|
// used to control FBW and limit the rate of climb
|
||||||
// -----------------------------------------------
|
// -----------------------------------------------
|
||||||
target_altitude = current_loc.alt; // PH: target_altitude = 200
|
target_altitude = current_loc.alt;
|
||||||
offset_altitude = next_WP.alt - prev_WP.alt; // PH: offset_altitude = 0
|
|
||||||
|
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
|
||||||
|
offset_altitude = next_WP.alt - prev_WP.alt;
|
||||||
|
else
|
||||||
|
offset_altitude = 0;
|
||||||
|
|
||||||
// zero out our loiter vals to watch for missed waypoints
|
// zero out our loiter vals to watch for missed waypoints
|
||||||
loiter_delta = 0;
|
loiter_delta = 0;
|
||||||
@ -223,7 +247,8 @@ void init_home()
|
|||||||
|
|
||||||
// ground altitude in centimeters for pressure alt calculations
|
// ground altitude in centimeters for pressure alt calculations
|
||||||
// ------------------------------------------------------------
|
// ------------------------------------------------------------
|
||||||
save_EEPROM_pressure();
|
g.ground_pressure.save();
|
||||||
|
|
||||||
|
|
||||||
// Save Home to EEPROM
|
// Save Home to EEPROM
|
||||||
// -------------------
|
// -------------------
|
||||||
|
|||||||
285
ArduCopterMega/commands_logic.pde
Normal file
285
ArduCopterMega/commands_logic.pde
Normal file
@ -0,0 +1,285 @@
|
|||||||
|
void
|
||||||
|
handle_no_commands()
|
||||||
|
{
|
||||||
|
switch (control_mode){
|
||||||
|
case LAND:
|
||||||
|
// don't get a new command
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
next_command = get_LOITER_home_wp();
|
||||||
|
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
|
||||||
|
//SendDebugln(next_command.id,DEC);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
handle_process_must(byte id)
|
||||||
|
{
|
||||||
|
// reset navigation integrators
|
||||||
|
// -------------------------
|
||||||
|
reset_I();
|
||||||
|
|
||||||
|
switch(id){
|
||||||
|
case MAV_CMD_NAV_TAKEOFF: // TAKEOFF!
|
||||||
|
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
||||||
|
takeoff_altitude = (int)next_command.alt;
|
||||||
|
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
||||||
|
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
||||||
|
next_WP.alt = current_loc.alt + takeoff_altitude;
|
||||||
|
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
||||||
|
//set_next_WP(&next_WP);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
|
||||||
|
break;
|
||||||
|
|
||||||
|
//case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint
|
||||||
|
// next_command.lat += home.lat; // offset from home location
|
||||||
|
// next_command.lng += home.lng; // offset from home location
|
||||||
|
|
||||||
|
// we've recalculated the WP so we need to set it again
|
||||||
|
// set_next_WP(&next_command);
|
||||||
|
// break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LAND: // LAND to Waypoint
|
||||||
|
velocity_land = 1000;
|
||||||
|
next_WP.lat = current_loc.lat;
|
||||||
|
next_WP.lng = current_loc.lng;
|
||||||
|
next_WP.alt = home.alt;
|
||||||
|
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
||||||
|
break;
|
||||||
|
|
||||||
|
/*
|
||||||
|
case MAV_CMD_ALTITUDE: //
|
||||||
|
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
||||||
|
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
||||||
|
break;
|
||||||
|
*/
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_TIME:
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||||
|
return_to_launch();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
handle_process_may(byte id)
|
||||||
|
{
|
||||||
|
switch(id){
|
||||||
|
|
||||||
|
case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint
|
||||||
|
delay_start = millis();
|
||||||
|
delay_timeout = next_command.lat;
|
||||||
|
break;
|
||||||
|
|
||||||
|
//case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy
|
||||||
|
// break;
|
||||||
|
|
||||||
|
case MAV_CMD_CONDITION_YAW:
|
||||||
|
// p1: bearing
|
||||||
|
// alt: speed
|
||||||
|
// lat: direction (-1,1),
|
||||||
|
// lng: rel (1) abs (0)
|
||||||
|
|
||||||
|
// target angle in degrees
|
||||||
|
command_yaw_start = nav_yaw; // current position
|
||||||
|
command_yaw_start_time = millis();
|
||||||
|
|
||||||
|
// which direction to turn
|
||||||
|
// 1 = clockwise, -1 = counterclockwise
|
||||||
|
command_yaw_dir = next_command.lat;
|
||||||
|
|
||||||
|
// 1 = Relative or 0 = Absolute
|
||||||
|
if (next_command.lng == 1) {
|
||||||
|
// relative
|
||||||
|
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
|
||||||
|
command_yaw_end += nav_yaw;
|
||||||
|
command_yaw_end = wrap_360(command_yaw_end);
|
||||||
|
}else{
|
||||||
|
// absolute
|
||||||
|
command_yaw_end = next_command.p1 * 100;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// if unspecified go 10° a second
|
||||||
|
if(command_yaw_speed == 0)
|
||||||
|
command_yaw_speed = 10;
|
||||||
|
|
||||||
|
// if unspecified go clockwise
|
||||||
|
if(command_yaw_dir == 0)
|
||||||
|
command_yaw_dir = 1;
|
||||||
|
|
||||||
|
// calculate the delta travel
|
||||||
|
if(command_yaw_dir == 1){
|
||||||
|
if(command_yaw_start > command_yaw_end){
|
||||||
|
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
|
||||||
|
}else{
|
||||||
|
command_yaw_delta = command_yaw_end - command_yaw_start;
|
||||||
|
}
|
||||||
|
}else{
|
||||||
|
if(command_yaw_start > command_yaw_end){
|
||||||
|
command_yaw_delta = command_yaw_start - command_yaw_end;
|
||||||
|
}else{
|
||||||
|
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
command_yaw_delta = wrap_360(command_yaw_delta);
|
||||||
|
|
||||||
|
// rate to turn deg per second - default is ten
|
||||||
|
command_yaw_speed = next_command.alt;
|
||||||
|
command_yaw_time = command_yaw_delta / command_yaw_speed;
|
||||||
|
//9000 turn in 10 seconds
|
||||||
|
//command_yaw_time = 9000/ 10 = 900° per second
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}}
|
||||||
|
|
||||||
|
void
|
||||||
|
handle_process_now(byte id)
|
||||||
|
{
|
||||||
|
switch(id){
|
||||||
|
case MAV_CMD_DO_SET_HOME:
|
||||||
|
init_home();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_DO_REPEAT_SERVO:
|
||||||
|
new_event(&next_command);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_DO_SET_SERVO:
|
||||||
|
//Serial.print("MAV_CMD_DO_SET_SERVO ");
|
||||||
|
//Serial.print(next_command.p1,DEC);
|
||||||
|
//Serial.print(" PWM: ");
|
||||||
|
//Serial.println(next_command.alt,DEC);
|
||||||
|
APM_RC.OutputCh(next_command.p1, next_command.alt);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_DO_SET_RELAY:
|
||||||
|
if (next_command.p1 == 0) {
|
||||||
|
relay_on();
|
||||||
|
} else if (next_command.p1 == 1) {
|
||||||
|
relay_off();
|
||||||
|
}else{
|
||||||
|
relay_toggle();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Verify commands
|
||||||
|
// ---------------
|
||||||
|
void verify_must()
|
||||||
|
{
|
||||||
|
switch(command_must_ID) {
|
||||||
|
|
||||||
|
/*case MAV_CMD_ALTITUDE:
|
||||||
|
if (abs(next_WP.alt - current_loc.alt) < 100){
|
||||||
|
command_must_index = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
*/
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
|
||||||
|
if (current_loc.alt > (next_WP.alt -100)){
|
||||||
|
command_must_index = 0;
|
||||||
|
takeoff_complete = true;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LAND:
|
||||||
|
// 10 - 9 = 1
|
||||||
|
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
|
||||||
|
old_alt = current_loc.alt;
|
||||||
|
if(velocity_land == 0){
|
||||||
|
land_complete = true;
|
||||||
|
command_must_index = 0;
|
||||||
|
}
|
||||||
|
update_crosstrack();
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
|
||||||
|
update_crosstrack();
|
||||||
|
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
|
||||||
|
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
|
||||||
|
//SendDebugln(command_must_index,DEC);
|
||||||
|
char message[50];
|
||||||
|
sprintf(message,"Reached Waypoint #%i",command_must_index);
|
||||||
|
gcs.send_text(SEVERITY_LOW,message);
|
||||||
|
|
||||||
|
// clear the command queue;
|
||||||
|
command_must_index = 0;
|
||||||
|
}
|
||||||
|
// add in a more complex case
|
||||||
|
// Doug to do
|
||||||
|
if(loiter_sum > 300){
|
||||||
|
send_message(SEVERITY_MEDIUM,"Missed WP");
|
||||||
|
command_must_index = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_TURNS: // LOITER N times
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||||
|
if (wp_distance <= g.waypoint_radius) {
|
||||||
|
gcs.send_text(SEVERITY_LOW,"Reached home");
|
||||||
|
command_must_index = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
//case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER
|
||||||
|
// break;
|
||||||
|
|
||||||
|
|
||||||
|
default:
|
||||||
|
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void verify_may()
|
||||||
|
{
|
||||||
|
float power;
|
||||||
|
switch(command_may_ID) {
|
||||||
|
|
||||||
|
case MAV_CMD_CONDITION_ANGLE:
|
||||||
|
if((millis() - command_yaw_start_time) > command_yaw_time){
|
||||||
|
command_must_index = 0;
|
||||||
|
nav_yaw = command_yaw_end;
|
||||||
|
}else{
|
||||||
|
// else we need to be at a certain place
|
||||||
|
// power is a ratio of the time : .5 = half done
|
||||||
|
power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
|
||||||
|
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_CONDITION_DELAY:
|
||||||
|
if ((millis() - delay_start) > delay_timeout){
|
||||||
|
command_may_index = 0;
|
||||||
|
delay_timeout = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
//case CMD_LAND_OPTIONS:
|
||||||
|
// break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@ -42,18 +42,7 @@ void load_next_command()
|
|||||||
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
|
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
|
||||||
//SendDebugln(g.waypoint_index,DEC);
|
//SendDebugln(g.waypoint_index,DEC);
|
||||||
|
|
||||||
|
handle_no_commands();
|
||||||
switch (control_mode){
|
|
||||||
case LAND:
|
|
||||||
// don't get a new command
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
next_command = get_LOITER_home_wp();
|
|
||||||
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
|
|
||||||
//SendDebugln(next_command.id,DEC);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -125,62 +114,7 @@ void process_must()
|
|||||||
// invalidate command so a new one is loaded
|
// invalidate command so a new one is loaded
|
||||||
// -----------------------------------------
|
// -----------------------------------------
|
||||||
next_command.id = 0;
|
next_command.id = 0;
|
||||||
|
handle_process_must(command_must_ID);
|
||||||
// reset navigation integrators
|
|
||||||
// -------------------------
|
|
||||||
reset_I();
|
|
||||||
|
|
||||||
switch(command_must_ID){
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_TAKEOFF: // TAKEOFF!
|
|
||||||
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
|
||||||
takeoff_altitude = (int)next_command.alt;
|
|
||||||
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
|
||||||
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
|
||||||
next_WP.alt = current_loc.alt + takeoff_altitude;
|
|
||||||
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
|
||||||
//set_next_WP(&next_WP);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
|
|
||||||
break;
|
|
||||||
|
|
||||||
//case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint
|
|
||||||
// next_command.lat += home.lat; // offset from home location
|
|
||||||
// next_command.lng += home.lng; // offset from home location
|
|
||||||
|
|
||||||
// we've recalculated the WP so we need to set it again
|
|
||||||
// set_next_WP(&next_command);
|
|
||||||
// break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LAND: // LAND to Waypoint
|
|
||||||
velocity_land = 1000;
|
|
||||||
next_WP.lat = current_loc.lat;
|
|
||||||
next_WP.lng = current_loc.lng;
|
|
||||||
next_WP.alt = home.alt;
|
|
||||||
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
|
||||||
break;
|
|
||||||
|
|
||||||
/*
|
|
||||||
case MAV_CMD_ALTITUDE: //
|
|
||||||
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
|
||||||
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
|
||||||
break;
|
|
||||||
*/
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_TIME:
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
|
||||||
return_to_launch();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void process_may()
|
void process_may()
|
||||||
@ -195,79 +129,8 @@ void process_may()
|
|||||||
|
|
||||||
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
|
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
|
||||||
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
||||||
// do the command
|
|
||||||
// --------------
|
|
||||||
switch(command_may_ID){
|
|
||||||
|
|
||||||
case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint
|
handle_process_may(command_may_ID);
|
||||||
delay_start = millis();
|
|
||||||
delay_timeout = next_command.lat;
|
|
||||||
break;
|
|
||||||
|
|
||||||
//case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy
|
|
||||||
// break;
|
|
||||||
|
|
||||||
case MAV_CMD_CONDITION_YAW:
|
|
||||||
// p1: bearing
|
|
||||||
// alt: speed
|
|
||||||
// lat: direction (-1,1),
|
|
||||||
// lng: rel (1) abs (0)
|
|
||||||
|
|
||||||
// target angle in degrees
|
|
||||||
command_yaw_start = nav_yaw; // current position
|
|
||||||
command_yaw_start_time = millis();
|
|
||||||
|
|
||||||
// which direction to turn
|
|
||||||
// 1 = clockwise, -1 = counterclockwise
|
|
||||||
command_yaw_dir = next_command.lat;
|
|
||||||
|
|
||||||
// 1 = Relative or 0 = Absolute
|
|
||||||
if (next_command.lng == 1) {
|
|
||||||
// relative
|
|
||||||
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
|
|
||||||
command_yaw_end += nav_yaw;
|
|
||||||
command_yaw_end = wrap_360(command_yaw_end);
|
|
||||||
} else {
|
|
||||||
// absolute
|
|
||||||
command_yaw_end = next_command.p1 * 100;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// if unspecified go 10° a second
|
|
||||||
if(command_yaw_speed == 0)
|
|
||||||
command_yaw_speed = 10;
|
|
||||||
|
|
||||||
// if unspecified go clockwise
|
|
||||||
if(command_yaw_dir == 0)
|
|
||||||
command_yaw_dir = 1;
|
|
||||||
|
|
||||||
// calculate the delta travel
|
|
||||||
if(command_yaw_dir == 1){
|
|
||||||
if(command_yaw_start > command_yaw_end){
|
|
||||||
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
|
|
||||||
}else{
|
|
||||||
command_yaw_delta = command_yaw_end - command_yaw_start;
|
|
||||||
}
|
|
||||||
}else{
|
|
||||||
if(command_yaw_start > command_yaw_end){
|
|
||||||
command_yaw_delta = command_yaw_start - command_yaw_end;
|
|
||||||
}else{
|
|
||||||
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
command_yaw_delta = wrap_360(command_yaw_delta);
|
|
||||||
|
|
||||||
// rate to turn deg per second - default is ten
|
|
||||||
command_yaw_speed = next_command.alt;
|
|
||||||
command_yaw_time = command_yaw_delta / command_yaw_speed;
|
|
||||||
//9000 turn in 10 seconds
|
|
||||||
//command_yaw_time = 9000/ 10 = 900° per second
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void process_now()
|
void process_now()
|
||||||
@ -284,187 +147,6 @@ void process_now()
|
|||||||
|
|
||||||
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
|
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
|
||||||
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
||||||
|
handle_process_now(id);
|
||||||
// do the command
|
|
||||||
// --------------
|
|
||||||
switch(id){
|
|
||||||
/*
|
|
||||||
case CMD_THROTTLE_CRUISE:
|
|
||||||
g.throttle_cruise = next_command.p1;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_RESET_HOME:
|
|
||||||
init_home();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_KP_GAIN:
|
|
||||||
//kp[next_command.p1] = next_command.alt/t5;
|
|
||||||
// todo save to EEPROM
|
|
||||||
break;
|
|
||||||
case CMD_KI_GAIN:
|
|
||||||
//ki[next_command.p1] = next_command.alt/t5;
|
|
||||||
// todo save to EEPROM
|
|
||||||
break;
|
|
||||||
case CMD_KD_GAIN:
|
|
||||||
//kd[next_command.p1] = next_command.alt/t5;
|
|
||||||
// todo save to EEPROM
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_KI_MAX:
|
|
||||||
//integrator_max[next_command.p1] = next_command.alt/t5;
|
|
||||||
// todo save to EEPROM
|
|
||||||
break;
|
|
||||||
|
|
||||||
//case CMD_KFF_GAIN:
|
|
||||||
// kff[next_command.p1] = next_command.alt/t5;
|
|
||||||
// todo save to EEPROM
|
|
||||||
// break;
|
|
||||||
|
|
||||||
//case CMD_RADIO_TRIM:
|
|
||||||
//radio_trim[next_command.p1] = next_command.alt;
|
|
||||||
//save_EEPROM_trims();
|
|
||||||
//break;
|
|
||||||
|
|
||||||
//case CMD_RADIO_MAX:
|
|
||||||
// radio_max[next_command.p1] = next_command.alt;
|
|
||||||
// save_EEPROM_radio_minmax();
|
|
||||||
// break;
|
|
||||||
|
|
||||||
//case CMD_RADIO_MIN:
|
|
||||||
// radio_min[next_command.p1] = next_command.alt;
|
|
||||||
// save_EEPROM_radio_minmax();
|
|
||||||
// break;
|
|
||||||
*/
|
|
||||||
|
|
||||||
case MAV_CMD_DO_SET_HOME:
|
|
||||||
init_home();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_DO_REPEAT_SERVO:
|
|
||||||
new_event(&next_command);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_DO_SET_SERVO:
|
|
||||||
//Serial.print("MAV_CMD_DO_SET_SERVO ");
|
|
||||||
//Serial.print(next_command.p1,DEC);
|
|
||||||
//Serial.print(" PWM: ");
|
|
||||||
//Serial.println(next_command.alt,DEC);
|
|
||||||
APM_RC.OutputCh(next_command.p1, next_command.alt);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_DO_SET_RELAY:
|
|
||||||
if (next_command.p1 == 0) {
|
|
||||||
relay_on();
|
|
||||||
} else if (next_command.p1 == 1) {
|
|
||||||
relay_off();
|
|
||||||
}else{
|
|
||||||
relay_toggle();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Verify commands
|
|
||||||
// ---------------
|
|
||||||
void verify_must()
|
|
||||||
{
|
|
||||||
|
|
||||||
switch(command_must_ID) {
|
|
||||||
|
|
||||||
/*case MAV_CMD_ALTITUDE:
|
|
||||||
if (abs(next_WP.alt - current_loc.alt) < 100){
|
|
||||||
command_must_index = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
*/
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
|
|
||||||
if (current_loc.alt > (next_WP.alt -100)){
|
|
||||||
command_must_index = 0;
|
|
||||||
takeoff_complete = true;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LAND:
|
|
||||||
// 10 - 9 = 1
|
|
||||||
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
|
|
||||||
old_alt = current_loc.alt;
|
|
||||||
if(velocity_land == 0){
|
|
||||||
land_complete = true;
|
|
||||||
command_must_index = 0;
|
|
||||||
}
|
|
||||||
update_crosstrack();
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
|
|
||||||
update_crosstrack();
|
|
||||||
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
|
|
||||||
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
|
|
||||||
//SendDebugln(command_must_index,DEC);
|
|
||||||
char message[50];
|
|
||||||
sprintf(message,"Reached Waypoint #%i",command_must_index);
|
|
||||||
gcs.send_text(SEVERITY_LOW,message);
|
|
||||||
|
|
||||||
// clear the command queue;
|
|
||||||
command_must_index = 0;
|
|
||||||
}
|
|
||||||
// add in a more complex case
|
|
||||||
// Doug to do
|
|
||||||
if(loiter_sum > 300){
|
|
||||||
send_message(SEVERITY_MEDIUM,"Missed WP");
|
|
||||||
command_must_index = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_TURNS: // LOITER N times
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
|
||||||
if (wp_distance <= g.waypoint_radius) {
|
|
||||||
gcs.send_text(SEVERITY_LOW,"Reached home");
|
|
||||||
command_must_index = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
//case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER
|
|
||||||
// break;
|
|
||||||
|
|
||||||
|
|
||||||
default:
|
|
||||||
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void verify_may()
|
|
||||||
{
|
|
||||||
float power;
|
|
||||||
switch(command_may_ID) {
|
|
||||||
|
|
||||||
case MAV_CMD_CONDITION_ANGLE:
|
|
||||||
if((millis() - command_yaw_start_time) > command_yaw_time){
|
|
||||||
command_must_index = 0;
|
|
||||||
nav_yaw = command_yaw_end;
|
|
||||||
}else{
|
|
||||||
// else we need to be at a certain place
|
|
||||||
// power is a ratio of the time : .5 = half done
|
|
||||||
power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
|
|
||||||
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_CONDITION_DELAY:
|
|
||||||
if ((millis() - delay_start) > delay_timeout){
|
|
||||||
command_may_index = 0;
|
|
||||||
delay_timeout = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
//case CMD_LAND_OPTIONS:
|
|
||||||
// break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -26,7 +26,7 @@
|
|||||||
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
||||||
/// change in your local copy of APM_Config.h.
|
/// change in your local copy of APM_Config.h.
|
||||||
///
|
///
|
||||||
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT
|
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
|
||||||
///
|
///
|
||||||
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
||||||
/// change in your local copy of APM_Config.h.
|
/// change in your local copy of APM_Config.h.
|
||||||
@ -42,24 +42,6 @@
|
|||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
// ENABLE_HIL
|
|
||||||
// Hardware in the loop output
|
|
||||||
//
|
|
||||||
#ifndef ENABLE_HIL
|
|
||||||
# define ENABLE_HIL DISABLED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
// GPS_PROTOCOL
|
|
||||||
//
|
|
||||||
// Note that this test must follow the HIL_PROTOCOL block as the HIL
|
|
||||||
// setup may override the GPS configuration.
|
|
||||||
//
|
|
||||||
#ifndef GPS_PROTOCOL
|
|
||||||
# define GCS_PROTOCOL GPS_PROTOCOL_AUTO
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// FRAME_CONFIG
|
// FRAME_CONFIG
|
||||||
@ -75,32 +57,81 @@
|
|||||||
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE
|
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
// AIRSPEED_SENSOR
|
||||||
|
// AIRSPEED_RATIO
|
||||||
|
//
|
||||||
|
#ifndef AIRSPEED_SENSOR
|
||||||
|
# define AIRSPEED_SENSOR DISABLED
|
||||||
|
#endif
|
||||||
|
#ifndef AIRSPEED_RATIO
|
||||||
|
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
// HIL_PROTOCOL OPTIONAL
|
||||||
|
// HIL_MODE OPTIONAL
|
||||||
|
// HIL_PORT OPTIONAL
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
|
||||||
|
|
||||||
|
# undef GPS_PROTOCOL
|
||||||
|
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
|
||||||
|
|
||||||
|
#ifndef HIL_PROTOCOL
|
||||||
|
# error Must define HIL_PROTOCOL if HIL_MODE is not disabled
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef HIL_PORT
|
||||||
|
# error Must define HIL_PORT if HIL_PROTOCOL is set
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
// If we are in XPlane, diasble the mag
|
||||||
|
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
|
||||||
|
|
||||||
|
// check xplane settings
|
||||||
|
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
|
||||||
|
|
||||||
|
// MAGNETOMETER not supported by XPLANE
|
||||||
|
# undef MAGNETOMETER
|
||||||
|
# define MAGNETOMETER DISABLED
|
||||||
|
|
||||||
|
# if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
|
||||||
|
# endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
// GPS_PROTOCOL
|
||||||
|
//
|
||||||
|
// Note that this test must follow the HIL_PROTOCOL block as the HIL
|
||||||
|
// setup may override the GPS configuration.
|
||||||
|
//
|
||||||
|
#ifndef GPS_PROTOCOL
|
||||||
|
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
|
||||||
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// GCS_PROTOCOL
|
// GCS_PROTOCOL
|
||||||
// GCS_PORT
|
// GCS_PORT
|
||||||
//
|
//
|
||||||
#ifndef GCS_PROTOCOL
|
#ifndef GCS_PROTOCOL
|
||||||
# define GCS_PROTOCOL GCS_PROTOCOL_STANDARD
|
# define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef GCS_PORT
|
|
||||||
# if (GCS_PROTOCOL == GCS_PROTOCOL_STANDARD) || (GCS_PROTOCOL == GCS_PROTOCOL_LEGACY)
|
|
||||||
# define GCS_PORT 3
|
|
||||||
# else
|
|
||||||
# define GCS_PORT 0
|
|
||||||
# endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// Serial port speeds.
|
// Serial port speeds.
|
||||||
//
|
//
|
||||||
#ifndef SERIAL0_BAUD
|
#ifndef SERIAL0_BAUD
|
||||||
# define SERIAL0_BAUD 38400
|
# define SERIAL0_BAUD 115200
|
||||||
#endif
|
#endif
|
||||||
#ifndef SERIAL3_BAUD
|
#ifndef SERIAL3_BAUD
|
||||||
# define SERIAL3_BAUD 115200
|
# define SERIAL3_BAUD 57600
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
@ -119,6 +150,7 @@
|
|||||||
#ifndef VOLT_DIV_RATIO
|
#ifndef VOLT_DIV_RATIO
|
||||||
# define VOLT_DIV_RATIO 3.0
|
# define VOLT_DIV_RATIO 3.0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef CURR_VOLT_DIV_RATIO
|
#ifndef CURR_VOLT_DIV_RATIO
|
||||||
# define CURR_VOLT_DIV_RATIO 15.7
|
# define CURR_VOLT_DIV_RATIO 15.7
|
||||||
#endif
|
#endif
|
||||||
@ -137,28 +169,15 @@
|
|||||||
# define INPUT_VOLTAGE 5.0
|
# define INPUT_VOLTAGE 5.0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// MAGNETOMETER
|
// MAGNETOMETER
|
||||||
#ifndef MAGORIENTATION
|
#ifndef MAGNETOMETER
|
||||||
# define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_FORWARD
|
# define MAGNETOMETER DISABLED
|
||||||
#endif
|
#endif
|
||||||
|
#ifndef MAG_ORIENTATION
|
||||||
// run the CLI tool to get this value
|
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||||
#ifndef MAGOFFSET
|
|
||||||
# define MAGOFFSET 0,0,0
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Declination is a correction factor between North Pole and real magnetic North. This is different on every location
|
|
||||||
// IF you want to use really accurate headholding and future navigation features, you should update this
|
|
||||||
// You can check Declination to your location from http://www.magnetic-declination.com/
|
|
||||||
#ifndef DECLINATION
|
|
||||||
# define DECLINATION 0
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
@ -366,7 +385,6 @@
|
|||||||
#ifndef PITCH_MAX
|
#ifndef PITCH_MAX
|
||||||
# define PITCH_MAX 36
|
# define PITCH_MAX 36
|
||||||
#endif
|
#endif
|
||||||
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
|
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
@ -427,8 +445,6 @@
|
|||||||
#ifndef XTRACK_ENTRY_ANGLE
|
#ifndef XTRACK_ENTRY_ANGLE
|
||||||
# define XTRACK_ENTRY_ANGLE 30 // deg
|
# define XTRACK_ENTRY_ANGLE 30 // deg
|
||||||
#endif
|
#endif
|
||||||
//# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100
|
|
||||||
//# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100
|
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
|||||||
@ -8,6 +8,7 @@ void read_control_switch()
|
|||||||
set_mode(g.flight_modes[switchPosition]);
|
set_mode(g.flight_modes[switchPosition]);
|
||||||
|
|
||||||
oldSwitchPosition = switchPosition;
|
oldSwitchPosition = switchPosition;
|
||||||
|
prev_WP = current_loc;
|
||||||
|
|
||||||
// reset navigation integrators
|
// reset navigation integrators
|
||||||
// -------------------------
|
// -------------------------
|
||||||
@ -40,8 +41,8 @@ void reset_control_switch()
|
|||||||
{
|
{
|
||||||
oldSwitchPosition = -1;
|
oldSwitchPosition = -1;
|
||||||
read_control_switch();
|
read_control_switch();
|
||||||
//Serial.print("MSG: reset_control_switch");
|
SendDebug("MSG: reset_control_switch");
|
||||||
//Serial.println(oldSwitchPosition , DEC);
|
SendDebugln(oldSwitchPosition , DEC);
|
||||||
}
|
}
|
||||||
|
|
||||||
void update_servo_switches()
|
void update_servo_switches()
|
||||||
@ -74,7 +75,7 @@ void read_trim_switch()
|
|||||||
if((millis() - trim_timer) > 2000){
|
if((millis() - trim_timer) > 2000){
|
||||||
imu.save();
|
imu.save();
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
// set the throttle nominal
|
// set the throttle nominal
|
||||||
if(g.rc_3.control_in > 50){
|
if(g.rc_3.control_in > 50){
|
||||||
g.throttle_cruise.set(g.rc_3.control_in);
|
g.throttle_cruise.set(g.rc_3.control_in);
|
||||||
@ -87,7 +88,7 @@ void read_trim_switch()
|
|||||||
// this is a test for Max's tri-copter
|
// this is a test for Max's tri-copter
|
||||||
if(g.frame_type == TRI_FRAME){
|
if(g.frame_type == TRI_FRAME){
|
||||||
g.rc_4.trim(); // yaw
|
g.rc_4.trim(); // yaw
|
||||||
g.rc_4.save_trim();
|
g.rc_4.save_eeprom();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
trim_flag = false;
|
trim_flag = false;
|
||||||
|
|||||||
@ -1,5 +1,26 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
|
|
||||||
|
// ACM:
|
||||||
|
// Motors
|
||||||
|
#define RIGHT CH_1
|
||||||
|
#define LEFT CH_2
|
||||||
|
#define FRONT CH_3
|
||||||
|
#define BACK CH_4
|
||||||
|
#define RIGHTFRONT CH_7
|
||||||
|
#define LEFTBACK CH_8
|
||||||
|
#define MAX_SERVO_OUTPUT 2700
|
||||||
|
|
||||||
|
// active altitude sensor
|
||||||
|
#define SONAR 0
|
||||||
|
#define BARO 1
|
||||||
|
|
||||||
|
// Frame types
|
||||||
|
#define PLUS_FRAME 0
|
||||||
|
#define X_FRAME 1
|
||||||
|
#define TRI_FRAME 2
|
||||||
|
#define HEXA_FRAME 3
|
||||||
|
|
||||||
// Internal defines, don't edit and expect things to work
|
// Internal defines, don't edit and expect things to work
|
||||||
// -------------------------------------------------------
|
// -------------------------------------------------------
|
||||||
|
|
||||||
@ -14,16 +35,6 @@
|
|||||||
#define T6 1000000
|
#define T6 1000000
|
||||||
#define T7 10000000
|
#define T7 10000000
|
||||||
|
|
||||||
// GPS baud rates
|
|
||||||
// --------------
|
|
||||||
#define NO_GPS 38400
|
|
||||||
#define NMEA_GPS 38400
|
|
||||||
#define EM406_GPS 57600
|
|
||||||
#define UBLOX_GPS 38400
|
|
||||||
#define ARDU_IMU 38400
|
|
||||||
#define MTK_GPS 38400
|
|
||||||
#define SIM_GPS 38400
|
|
||||||
|
|
||||||
// GPS type codes - use the names, not the numbers
|
// GPS type codes - use the names, not the numbers
|
||||||
#define GPS_PROTOCOL_NONE -1
|
#define GPS_PROTOCOL_NONE -1
|
||||||
#define GPS_PROTOCOL_NMEA 0
|
#define GPS_PROTOCOL_NMEA 0
|
||||||
@ -52,8 +63,6 @@
|
|||||||
#define CH_THROTTLE CH_3
|
#define CH_THROTTLE CH_3
|
||||||
#define CH_RUDDER CH_4
|
#define CH_RUDDER CH_4
|
||||||
#define CH_YAW CH_4
|
#define CH_YAW CH_4
|
||||||
#define CH_AUX CH_5
|
|
||||||
#define CH_AUX2 CH_6
|
|
||||||
|
|
||||||
// HIL enumerations
|
// HIL enumerations
|
||||||
#define HIL_PROTOCOL_XPLANE 1
|
#define HIL_PROTOCOL_XPLANE 1
|
||||||
@ -63,38 +72,14 @@
|
|||||||
#define HIL_MODE_ATTITUDE 1
|
#define HIL_MODE_ATTITUDE 1
|
||||||
#define HIL_MODE_SENSORS 2
|
#define HIL_MODE_SENSORS 2
|
||||||
|
|
||||||
// motors
|
|
||||||
#define RIGHT CH_1
|
|
||||||
#define LEFT CH_2
|
|
||||||
#define FRONT CH_3
|
|
||||||
#define BACK CH_4
|
|
||||||
|
|
||||||
#define RIGHTFRONT CH_7
|
|
||||||
#define LEFTBACK CH_8
|
|
||||||
|
|
||||||
#define MAX_SERVO_OUTPUT 2700
|
|
||||||
|
|
||||||
#define SONAR 0
|
|
||||||
#define BARO 1
|
|
||||||
|
|
||||||
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
|
|
||||||
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
|
|
||||||
#define WP_SIZE 14
|
|
||||||
|
|
||||||
// GCS enumeration
|
// GCS enumeration
|
||||||
#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol
|
#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol
|
||||||
#define GCS_PROTOCOL_SPECIAL 1 // special test protocol (?)
|
#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol
|
||||||
#define GCS_PROTOCOL_LEGACY 2 // legacy ArduPilot protocol
|
#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation
|
||||||
#define GCS_PROTOCOL_XPLANE 3 // X-Plane HIL simulation
|
#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal
|
||||||
#define GCS_PROTOCOL_IMU 4 // ArdiPilot IMU output
|
#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol
|
||||||
#define GCS_PROTOCOL_JASON 5 // Jason's special secret GCS protocol
|
|
||||||
#define GCS_PROTOCOL_NONE -1 // No GCS output
|
#define GCS_PROTOCOL_NONE -1 // No GCS output
|
||||||
|
|
||||||
#define PLUS_FRAME 0
|
|
||||||
#define X_FRAME 1
|
|
||||||
#define TRI_FRAME 2
|
|
||||||
#define HEXA_FRAME 3
|
|
||||||
|
|
||||||
// Auto Pilot modes
|
// Auto Pilot modes
|
||||||
// ----------------
|
// ----------------
|
||||||
#define STABILIZE 0 // hold level position
|
#define STABILIZE 0 // hold level position
|
||||||
@ -102,15 +87,17 @@
|
|||||||
#define ALT_HOLD 2 // AUTO control
|
#define ALT_HOLD 2 // AUTO control
|
||||||
#define FBW 3 // AUTO control
|
#define FBW 3 // AUTO control
|
||||||
#define AUTO 4 // AUTO control
|
#define AUTO 4 // AUTO control
|
||||||
#define POSITION_HOLD 5 // Hold a single location
|
#define LOITER 5 // AUTO control
|
||||||
#define RTL 6 // AUTO control
|
#define POSITION_HOLD 6 // Hold a single location
|
||||||
#define TAKEOFF 7 // controlled decent rate
|
#define RTL 7 // AUTO control
|
||||||
#define LAND 8 // controlled decent rate
|
#define TAKEOFF 8 // controlled decent rate
|
||||||
#define NUM_MODES 9
|
#define LAND 9 // controlled decent rate
|
||||||
|
#define NUM_MODES 10
|
||||||
|
|
||||||
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
|
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
|
||||||
#define CMD_BLANK 0x00 // there is no command stored in the mem location requested
|
#define CMD_BLANK 0x00 // there is no command stored in the mem location requested
|
||||||
|
|
||||||
|
|
||||||
//repeating events
|
//repeating events
|
||||||
#define NO_REPEAT 0
|
#define NO_REPEAT 0
|
||||||
#define CH_4_TOGGLE 1
|
#define CH_4_TOGGLE 1
|
||||||
@ -183,7 +170,6 @@
|
|||||||
#define SEVERITY_HIGH 3
|
#define SEVERITY_HIGH 3
|
||||||
#define SEVERITY_CRITICAL 4
|
#define SEVERITY_CRITICAL 4
|
||||||
|
|
||||||
|
|
||||||
// Logging parameters
|
// Logging parameters
|
||||||
#define LOG_INDEX_MSG 0xF0
|
#define LOG_INDEX_MSG 0xF0
|
||||||
#define LOG_ATTITUDE_MSG 0x01
|
#define LOG_ATTITUDE_MSG 0x01
|
||||||
@ -211,6 +197,18 @@
|
|||||||
#define MASK_LOG_CMD (1<<8)
|
#define MASK_LOG_CMD (1<<8)
|
||||||
#define MASK_LOG_CUR (1<<9)
|
#define MASK_LOG_CUR (1<<9)
|
||||||
|
|
||||||
|
// bits in log_bitmask
|
||||||
|
#define LOGBIT_ATTITUDE_FAST (1<<0)
|
||||||
|
#define LOGBIT_ATTITUDE_MED (1<<1)
|
||||||
|
#define LOGBIT_GPS (1<<2)
|
||||||
|
#define LOGBIT_PM (1<<3)
|
||||||
|
#define LOGBIT_CTUN (1<<4)
|
||||||
|
#define LOGBIT_NTUN (1<<5)
|
||||||
|
#define LOGBIT_MODE (1<<6)
|
||||||
|
#define LOGBIT_RAW (1<<7)
|
||||||
|
#define LOGBIT_CMD (1<<8)
|
||||||
|
#define LOGBIT_CURRENT (1<<9)
|
||||||
|
|
||||||
// Waypoint Modes
|
// Waypoint Modes
|
||||||
// ----------------
|
// ----------------
|
||||||
#define ABS_WP 0
|
#define ABS_WP 0
|
||||||
@ -229,6 +227,10 @@
|
|||||||
#define EVENT_LOADED_WAYPOINT 3
|
#define EVENT_LOADED_WAYPOINT 3
|
||||||
#define EVENT_LOOP 4
|
#define EVENT_LOOP 4
|
||||||
|
|
||||||
|
// Climb rate calculations
|
||||||
|
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
|
||||||
|
|
||||||
|
|
||||||
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
|
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
|
||||||
|
|
||||||
#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO
|
#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO
|
||||||
@ -257,24 +259,9 @@
|
|||||||
#define B_LED_PIN 36
|
#define B_LED_PIN 36
|
||||||
#define C_LED_PIN 35
|
#define C_LED_PIN 35
|
||||||
|
|
||||||
// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
|
|
||||||
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
|
|
||||||
// Tested value : 418
|
|
||||||
|
|
||||||
|
|
||||||
// EEPROM addresses
|
// EEPROM addresses
|
||||||
#define EEPROM_MAX_ADDR 4096
|
#define EEPROM_MAX_ADDR 4096
|
||||||
|
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
|
||||||
|
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
|
||||||
// bits in log_bitmask
|
#define WP_SIZE 14
|
||||||
#define LOGBIT_ATTITUDE_FAST (1<<0)
|
|
||||||
#define LOGBIT_ATTITUDE_MED (1<<1)
|
|
||||||
#define LOGBIT_GPS (1<<2)
|
|
||||||
#define LOGBIT_PM (1<<3)
|
|
||||||
#define LOGBIT_CTUN (1<<4)
|
|
||||||
#define LOGBIT_NTUN (1<<5)
|
|
||||||
#define LOGBIT_MODE (1<<6)
|
|
||||||
#define LOGBIT_RAW (1<<7)
|
|
||||||
#define LOGBIT_CMD (1<<8)
|
|
||||||
#define LOGBIT_CURRENT (1<<9)
|
|
||||||
|
|
||||||
|
|||||||
@ -97,11 +97,6 @@ set_servos_4()
|
|||||||
//Serial.println("TRI_FRAME");
|
//Serial.println("TRI_FRAME");
|
||||||
// Tri-copter power distribution
|
// Tri-copter power distribution
|
||||||
|
|
||||||
float temp = cos_pitch_x * cos_roll_x;
|
|
||||||
temp = 2.0 - constrain(temp, .7, 1.0);
|
|
||||||
return temp;
|
|
||||||
}
|
|
||||||
|
|
||||||
int roll_out = (float)g.rc_1.pwm_out * .866;
|
int roll_out = (float)g.rc_1.pwm_out * .866;
|
||||||
int pitch_out = g.rc_2.pwm_out / 2;
|
int pitch_out = g.rc_2.pwm_out / 2;
|
||||||
|
|
||||||
|
|||||||
@ -7,8 +7,7 @@ void navigate()
|
|||||||
{
|
{
|
||||||
// do not navigate with corrupt data
|
// do not navigate with corrupt data
|
||||||
// ---------------------------------
|
// ---------------------------------
|
||||||
if (g_gps->fix == 0)
|
if (g_gps->fix == 0){
|
||||||
{
|
|
||||||
g_gps->new_data = false;
|
g_gps->new_data = false;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -19,10 +18,10 @@ void navigate()
|
|||||||
|
|
||||||
// waypoint distance from plane
|
// waypoint distance from plane
|
||||||
// ----------------------------
|
// ----------------------------
|
||||||
GPS_wp_distance = getDistance(¤t_loc, &next_WP);
|
wp_distance = getDistance(¤t_loc, &next_WP);
|
||||||
|
|
||||||
if (GPS_wp_distance < 0){
|
if (wp_distance < 0){
|
||||||
send_message(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
|
gcs.send_text(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
|
||||||
//Serial.println(wp_distance,DEC);
|
//Serial.println(wp_distance,DEC);
|
||||||
//print_current_waypoints();
|
//print_current_waypoints();
|
||||||
return;
|
return;
|
||||||
@ -32,10 +31,21 @@ void navigate()
|
|||||||
// --------------------------------------------
|
// --------------------------------------------
|
||||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||||
|
|
||||||
// nav_bearing will includes xtrack correction
|
// nav_bearing will includes xtrac correction
|
||||||
// -------------------------------------------
|
// ------------------------------------------
|
||||||
nav_bearing = target_bearing;
|
nav_bearing = target_bearing;
|
||||||
|
|
||||||
|
// check if we have missed the WP
|
||||||
|
loiter_delta = (target_bearing - old_target_bearing)/100;
|
||||||
|
|
||||||
|
// reset the old value
|
||||||
|
old_target_bearing = target_bearing;
|
||||||
|
|
||||||
|
// wrap values
|
||||||
|
if (loiter_delta > 180) loiter_delta -= 360;
|
||||||
|
if (loiter_delta < -180) loiter_delta += 360;
|
||||||
|
loiter_sum += abs(loiter_delta);
|
||||||
|
|
||||||
// control mode specific updates to nav_bearing
|
// control mode specific updates to nav_bearing
|
||||||
// --------------------------------------------
|
// --------------------------------------------
|
||||||
update_navigation();
|
update_navigation();
|
||||||
@ -56,14 +66,6 @@ void calc_nav()
|
|||||||
pitch_max = 22° (2200)
|
pitch_max = 22° (2200)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
//temp = dcm.get_dcm_matrix();
|
|
||||||
//yawvector.y = temp.b.x; // cos
|
|
||||||
//yawvector.x = temp.a.x; // sin
|
|
||||||
//yawvector.normalize();
|
|
||||||
|
|
||||||
//cos_yaw_x = yawvector.y; // 0
|
|
||||||
//sin_yaw_y = yawvector.x; // 1
|
|
||||||
|
|
||||||
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 50 - 30 = 20 pitch right
|
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 50 - 30 = 20 pitch right
|
||||||
lat_error = next_WP.lat - current_loc.lat; // 50 - 30 = 20 pitch up
|
lat_error = next_WP.lat - current_loc.lat; // 50 - 30 = 20 pitch up
|
||||||
|
|
||||||
@ -88,46 +90,12 @@ void calc_nav()
|
|||||||
nav_pitch = constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
|
nav_pitch = constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
void verify_missed_wp()
|
|
||||||
{
|
|
||||||
// check if we have missed the WP
|
|
||||||
loiter_delta = (target_bearing - old_target_bearing) / 100;
|
|
||||||
|
|
||||||
// reset the old value
|
|
||||||
old_target_bearing = target_bearing;
|
|
||||||
|
|
||||||
// wrap values
|
|
||||||
if (loiter_delta > 170) loiter_delta -= 360;
|
|
||||||
if (loiter_delta < -170) loiter_delta += 360;
|
|
||||||
loiter_sum += abs(loiter_delta);
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
|
|
||||||
void calc_bearing_error()
|
void calc_bearing_error()
|
||||||
{
|
{
|
||||||
bearing_error = nav_bearing - dcm.yaw_sensor;
|
bearing_error = nav_bearing - dcm.yaw_sensor;
|
||||||
bearing_error = wrap_180(bearing_error);
|
bearing_error = wrap_180(bearing_error);
|
||||||
}
|
}
|
||||||
|
|
||||||
void calc_distance_error()
|
|
||||||
{
|
|
||||||
wp_distance = GPS_wp_distance;
|
|
||||||
|
|
||||||
// this wants to work only while moving, but it should filter out jumpy GPS reads
|
|
||||||
// scale gs to whole deg (50hz / 100) scale bearing error down to whole deg
|
|
||||||
//distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error / 100));
|
|
||||||
//distance_estimate -= distance_gain * (float)(distance_estimate - GPS_wp_distance);
|
|
||||||
//wp_distance = distance_estimate;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*void calc_airspeed_errors()
|
|
||||||
{
|
|
||||||
//airspeed_error = airspeed_cruise - airspeed;
|
|
||||||
//airspeed_energy_error = (long)(((long)airspeed_cruise * (long)airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
|
|
||||||
} */
|
|
||||||
|
|
||||||
// calculated at 50 hz
|
|
||||||
void calc_altitude_error()
|
void calc_altitude_error()
|
||||||
{
|
{
|
||||||
if(control_mode == AUTO && offset_altitude != 0) {
|
if(control_mode == AUTO && offset_altitude != 0) {
|
||||||
@ -140,19 +108,13 @@ void calc_altitude_error()
|
|||||||
}else{
|
}else{
|
||||||
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
|
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
|
||||||
}
|
}
|
||||||
} else {
|
}else{
|
||||||
target_altitude = next_WP.alt;
|
target_altitude = next_WP.alt;
|
||||||
}
|
}
|
||||||
|
|
||||||
altitude_error = target_altitude - current_loc.alt;
|
altitude_error = target_altitude - current_loc.alt;
|
||||||
|
|
||||||
//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// target_altitude = current_loc.alt; // PH: target_altitude = -200
|
|
||||||
// offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0
|
|
||||||
|
|
||||||
|
|
||||||
long wrap_360(long error)
|
long wrap_360(long error)
|
||||||
{
|
{
|
||||||
if (error > 36000) error -= 36000;
|
if (error > 36000) error -= 36000;
|
||||||
@ -167,18 +129,33 @@ long wrap_180(long error)
|
|||||||
return error;
|
return error;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
// disabled for now
|
|
||||||
void update_loiter()
|
void update_loiter()
|
||||||
{
|
{
|
||||||
loiter_delta = (target_bearing - old_target_bearing) / 100;
|
float power;
|
||||||
// reset the old value
|
|
||||||
old_target_bearing = target_bearing;
|
if(wp_distance <= g.loiter_radius){
|
||||||
// wrap values
|
power = float(wp_distance) / float(g.loiter_radius);
|
||||||
if (loiter_delta > 170) loiter_delta -= 360;
|
nav_bearing += (int)(9000.0 * (2.0 + power));
|
||||||
if (loiter_delta < -170) loiter_delta += 360;
|
|
||||||
loiter_sum += loiter_delta;
|
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
|
||||||
} */
|
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
|
||||||
|
power = constrain(power, 0, 1);
|
||||||
|
nav_bearing -= power * 9000;
|
||||||
|
|
||||||
|
}else{
|
||||||
|
update_crosstrack();
|
||||||
|
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wp_distance < g.loiter_radius){
|
||||||
|
nav_bearing += 9000;
|
||||||
|
}else{
|
||||||
|
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
|
||||||
|
}
|
||||||
|
|
||||||
|
update_crosstrack;
|
||||||
|
nav_bearing = wrap_360(nav_bearing);
|
||||||
|
}
|
||||||
|
|
||||||
void update_crosstrack(void)
|
void update_crosstrack(void)
|
||||||
{
|
{
|
||||||
@ -196,7 +173,7 @@ void reset_crosstrack()
|
|||||||
crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following
|
crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following
|
||||||
}
|
}
|
||||||
|
|
||||||
int get_altitude_above_home(void)
|
long get_altitude_above_home(void)
|
||||||
{
|
{
|
||||||
// This is the altitude above the home location
|
// This is the altitude above the home location
|
||||||
// The GPS gives us altitude at Sea Level
|
// The GPS gives us altitude at Sea Level
|
||||||
|
|||||||
@ -2,26 +2,19 @@ void init_rc_in()
|
|||||||
{
|
{
|
||||||
read_EEPROM_radio(); // read Radio limits
|
read_EEPROM_radio(); // read Radio limits
|
||||||
g.rc_1.set_angle(4500);
|
g.rc_1.set_angle(4500);
|
||||||
g.rc_1.dead_zone = 60; // 60 = .6 degrees
|
|
||||||
g.rc_2.set_angle(4500);
|
g.rc_2.set_angle(4500);
|
||||||
g.rc_2.dead_zone = 60;
|
|
||||||
g.rc_3.set_range(0,1000);
|
g.rc_3.set_range(0,1000);
|
||||||
g.rc_3.dead_zone = 20;
|
|
||||||
g.rc_3.scale_output = .9;
|
g.rc_3.scale_output = .9;
|
||||||
g.rc_4.set_angle(6000);
|
g.rc_4.set_angle(6000);
|
||||||
|
|
||||||
|
g.rc_1.dead_zone = 60; // 60 = .6 degrees
|
||||||
|
g.rc_2.dead_zone = 60;
|
||||||
|
g.rc_3.dead_zone = 20;
|
||||||
g.rc_4.dead_zone = 500;
|
g.rc_4.dead_zone = 500;
|
||||||
|
|
||||||
g.rc_5.set_range(0,1000);
|
g.rc_5.set_range(0,1000);
|
||||||
g.rc_5.set_filter(false);
|
g.rc_5.set_filter(false);
|
||||||
|
|
||||||
// for kP values
|
|
||||||
//g.rc_6.set_range(200,800);
|
|
||||||
//g.rc_6.set_range(0,1800); // for faking GPS
|
|
||||||
g.rc_6.set_range(0,1000);
|
g.rc_6.set_range(0,1000);
|
||||||
|
|
||||||
// for camera angles
|
|
||||||
//g.rc_6.set_angle(4500);
|
|
||||||
//g.rc_6.dead_zone = 60;
|
|
||||||
|
|
||||||
g.rc_7.set_range(0,1000);
|
g.rc_7.set_range(0,1000);
|
||||||
g.rc_8.set_range(0,1000);
|
g.rc_8.set_range(0,1000);
|
||||||
}
|
}
|
||||||
@ -66,6 +59,50 @@ void read_radio()
|
|||||||
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
|
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
void throttle_failsafe(uint16_t pwm)
|
||||||
|
{
|
||||||
|
if(g.throttle_fs_enabled == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
//check for failsafe and debounce funky reads
|
||||||
|
// ------------------------------------------
|
||||||
|
if (pwm < g.throttle_fs_value){
|
||||||
|
// we detect a failsafe from radio
|
||||||
|
// throttle has dropped below the mark
|
||||||
|
failsafeCounter++;
|
||||||
|
if (failsafeCounter == 9){
|
||||||
|
SendDebug("MSG FS ON ");
|
||||||
|
SendDebugln(pwm, DEC);
|
||||||
|
}else if(failsafeCounter == 10) {
|
||||||
|
ch3_failsafe = true;
|
||||||
|
//set_failsafe(true);
|
||||||
|
//failsafeCounter = 10;
|
||||||
|
}else if (failsafeCounter > 10){
|
||||||
|
failsafeCounter = 11;
|
||||||
|
}
|
||||||
|
|
||||||
|
}else if(failsafeCounter > 0){
|
||||||
|
// we are no longer in failsafe condition
|
||||||
|
// but we need to recover quickly
|
||||||
|
failsafeCounter--;
|
||||||
|
if (failsafeCounter > 3){
|
||||||
|
failsafeCounter = 3;
|
||||||
|
}
|
||||||
|
if (failsafeCounter == 1){
|
||||||
|
SendDebug("MSG FS OFF ");
|
||||||
|
SendDebugln(pwm, DEC);
|
||||||
|
}else if(failsafeCounter == 0) {
|
||||||
|
ch3_failsafe = false;
|
||||||
|
//set_failsafe(false);
|
||||||
|
//failsafeCounter = -1;
|
||||||
|
}else if (failsafeCounter <0){
|
||||||
|
failsafeCounter = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
void trim_radio()
|
void trim_radio()
|
||||||
{
|
{
|
||||||
for (byte i = 0; i < 30; i++){
|
for (byte i = 0; i < 30; i++){
|
||||||
@ -76,9 +113,9 @@ void trim_radio()
|
|||||||
g.rc_2.trim(); // pitch
|
g.rc_2.trim(); // pitch
|
||||||
g.rc_4.trim(); // yaw
|
g.rc_4.trim(); // yaw
|
||||||
|
|
||||||
g.rc_1.save_trim();
|
g.rc_1.save_eeprom();
|
||||||
g.rc_2.save_trim();
|
g.rc_2.save_eeprom();
|
||||||
g.rc_4.save_trim();
|
g.rc_4.save_eeprom();
|
||||||
}
|
}
|
||||||
|
|
||||||
void trim_yaw()
|
void trim_yaw()
|
||||||
@ -88,3 +125,4 @@ void trim_yaw()
|
|||||||
}
|
}
|
||||||
g.rc_4.trim(); // yaw
|
g.rc_4.trim(); // yaw
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -1,7 +1,8 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
void ReadSCP1000(void) {}
|
void ReadSCP1000(void) {}
|
||||||
|
|
||||||
|
void init_barometer(void)
|
||||||
void init_pressure_ground(void)
|
|
||||||
{
|
{
|
||||||
for(int i = 0; i < 300; i++){ // We take some readings...
|
for(int i = 0; i < 300; i++){ // We take some readings...
|
||||||
delay(20);
|
delay(20);
|
||||||
@ -12,8 +13,10 @@ void init_pressure_ground(void)
|
|||||||
abs_pressure = barometer.Press;
|
abs_pressure = barometer.Press;
|
||||||
}
|
}
|
||||||
|
|
||||||
long
|
// Sensors are not available in HIL_MODE_ATTITUDE
|
||||||
read_barometer(void)
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
long read_barometer(void)
|
||||||
{
|
{
|
||||||
float x, scaling, temp;
|
float x, scaling, temp;
|
||||||
|
|
||||||
@ -24,6 +27,7 @@ read_barometer(void)
|
|||||||
scaling = (float)ground_pressure / (float)abs_pressure;
|
scaling = (float)ground_pressure / (float)abs_pressure;
|
||||||
temp = ((float)ground_temperature / 10.0f) + 273.15f;
|
temp = ((float)ground_temperature / 10.0f) + 273.15f;
|
||||||
x = log(scaling) * temp * 29271.267f;
|
x = log(scaling) * temp * 29271.267f;
|
||||||
|
|
||||||
return (x / 10);
|
return (x / 10);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -33,6 +37,10 @@ void read_airspeed(void)
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#if BATTERY_EVENT == 1
|
#if BATTERY_EVENT == 1
|
||||||
void read_battery(void)
|
void read_battery(void)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -102,26 +102,6 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
// note, cannot actually return here
|
// note, cannot actually return here
|
||||||
return(0);
|
return(0);
|
||||||
|
|
||||||
//zero_eeprom();
|
|
||||||
//default_gains();
|
|
||||||
|
|
||||||
|
|
||||||
// setup default values
|
|
||||||
/*
|
|
||||||
default_waypoint_info();
|
|
||||||
default_nav();
|
|
||||||
default_alt_hold();
|
|
||||||
default_frame();
|
|
||||||
default_flight_modes();
|
|
||||||
default_throttle();
|
|
||||||
default_logs();
|
|
||||||
default_current();
|
|
||||||
print_done();
|
|
||||||
*/
|
|
||||||
// finish
|
|
||||||
// ------
|
|
||||||
//return(0);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Perform radio setup.
|
// Perform radio setup.
|
||||||
@ -191,14 +171,9 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
|||||||
g.rc_8.update_min_max();
|
g.rc_8.update_min_max();
|
||||||
|
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
//g.rc_3.radio_max += 250;
|
|
||||||
Serial.flush();
|
Serial.flush();
|
||||||
|
|
||||||
save_EEPROM_radio();
|
save_EEPROM_radio();
|
||||||
//delay(100);
|
|
||||||
// double checking
|
|
||||||
//read_EEPROM_radio();
|
|
||||||
//print_radio_values();
|
|
||||||
print_done();
|
print_done();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -445,7 +420,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
|||||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||||
g.compass_enabled = false;
|
g.compass_enabled = false;
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
|
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
|
||||||
report_compass();
|
report_compass();
|
||||||
return 0;
|
return 0;
|
||||||
@ -471,7 +446,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
|
|||||||
} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
|
} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
|
||||||
g.frame_type = HEXA_FRAME;
|
g.frame_type = HEXA_FRAME;
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
|
Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
|
||||||
report_frame();
|
report_frame();
|
||||||
return 0;
|
return 0;
|
||||||
@ -486,23 +461,20 @@ static int8_t
|
|||||||
setup_current(uint8_t argc, const Menu::arg *argv)
|
setup_current(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
||||||
g.current_enabled.set(true);
|
g.current_enabled.set_and_save(true);
|
||||||
save_EEPROM_mag();
|
|
||||||
|
|
||||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||||
g.current_enabled.set(false);
|
g.current_enabled.set_and_save(false);
|
||||||
save_EEPROM_mag();
|
|
||||||
|
|
||||||
} else if(argv[1].i > 10){
|
} else if(argv[1].i > 10){
|
||||||
g.milliamp_hours = argv[1].i;
|
g.milliamp_hours.set_and_save(argv[1].i);
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
|
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
|
||||||
report_current();
|
report_current();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
save_EEPROM_current();
|
|
||||||
report_current();
|
report_current();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
@ -516,7 +488,7 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
|
|||||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||||
g.sonar_enabled.set_and_save(false);
|
g.sonar_enabled.set_and_save(false);
|
||||||
|
|
||||||
} else {
|
}else{
|
||||||
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
||||||
report_sonar();
|
report_sonar();
|
||||||
return 0;
|
return 0;
|
||||||
@ -604,7 +576,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
|
|
||||||
/***************************************************************************/
|
/***************************************************************************/
|
||||||
// CLI utilities
|
// CLI defaults
|
||||||
/***************************************************************************/
|
/***************************************************************************/
|
||||||
|
|
||||||
void default_waypoint_info()
|
void default_waypoint_info()
|
||||||
@ -769,7 +741,7 @@ default_gains()
|
|||||||
|
|
||||||
|
|
||||||
/***************************************************************************/
|
/***************************************************************************/
|
||||||
// CLI utilities
|
// CLI reports
|
||||||
/***************************************************************************/
|
/***************************************************************************/
|
||||||
|
|
||||||
void report_current()
|
void report_current()
|
||||||
@ -938,7 +910,6 @@ void report_compass()
|
|||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void report_flight_modes()
|
void report_flight_modes()
|
||||||
{
|
{
|
||||||
//print_blanks(2);
|
//print_blanks(2);
|
||||||
@ -1006,9 +977,7 @@ print_divider(void)
|
|||||||
Serial.println("");
|
Serial.println("");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int8_t
|
||||||
// for reading in vales for mode switch
|
|
||||||
boolean
|
|
||||||
radio_input_switch(void)
|
radio_input_switch(void)
|
||||||
{
|
{
|
||||||
static int8_t bouncer = 0;
|
static int8_t bouncer = 0;
|
||||||
@ -1028,7 +997,7 @@ radio_input_switch(void)
|
|||||||
|
|
||||||
if (bouncer == 1 || bouncer == -1) {
|
if (bouncer == 1 || bouncer == -1) {
|
||||||
return bouncer;
|
return bouncer;
|
||||||
} else {
|
}else{
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -1044,7 +1013,6 @@ void zero_eeprom(void)
|
|||||||
Serial.printf_P(PSTR("done\n"));
|
Serial.printf_P(PSTR("done\n"));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void print_enabled(boolean b)
|
void print_enabled(boolean b)
|
||||||
{
|
{
|
||||||
if(b)
|
if(b)
|
||||||
|
|||||||
@ -103,7 +103,7 @@ void init_ardupilot()
|
|||||||
g.format_version.set_and_save(Parameters::k_format_version);
|
g.format_version.set_and_save(Parameters::k_format_version);
|
||||||
|
|
||||||
Serial.println_P(PSTR("done."));
|
Serial.println_P(PSTR("done."));
|
||||||
} else {
|
}else{
|
||||||
unsigned long before = micros();
|
unsigned long before = micros();
|
||||||
// Load all auto-loaded EEPROM variables
|
// Load all auto-loaded EEPROM variables
|
||||||
AP_Var::load_all();
|
AP_Var::load_all();
|
||||||
@ -228,7 +228,7 @@ void startup_ground(void)
|
|||||||
|
|
||||||
// read Baro pressure at ground
|
// read Baro pressure at ground
|
||||||
//-----------------------------
|
//-----------------------------
|
||||||
init_pressure_ground();
|
init_barometer();
|
||||||
|
|
||||||
// initialize commands
|
// initialize commands
|
||||||
// -------------------
|
// -------------------
|
||||||
@ -344,7 +344,7 @@ void update_GPS_light(void)
|
|||||||
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
|
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
|
||||||
if (GPS_light){
|
if (GPS_light){
|
||||||
digitalWrite(C_LED_PIN, LOW);
|
digitalWrite(C_LED_PIN, LOW);
|
||||||
} else {
|
}else{
|
||||||
digitalWrite(C_LED_PIN, HIGH);
|
digitalWrite(C_LED_PIN, HIGH);
|
||||||
}
|
}
|
||||||
g_gps->valid_read = false;
|
g_gps->valid_read = false;
|
||||||
|
|||||||
@ -1,3 +1,5 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
// These are function definitions so the Menu can be constructed before the functions
|
// These are function definitions so the Menu can be constructed before the functions
|
||||||
// are defined below. Order matters to the compiler.
|
// 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_pwm(uint8_t argc, const Menu::arg *argv);
|
||||||
@ -8,8 +10,8 @@ static int8_t test_fbw(uint8_t argc, const Menu::arg *argv);
|
|||||||
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_dcm(uint8_t argc, const Menu::arg *argv);
|
//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_omega(uint8_t argc, const Menu::arg *argv);
|
//static int8_t test_omega(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||||
@ -47,8 +49,8 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
|||||||
{"gps", test_gps},
|
{"gps", test_gps},
|
||||||
{"adc", test_adc},
|
{"adc", test_adc},
|
||||||
{"imu", test_imu},
|
{"imu", test_imu},
|
||||||
{"dcm", test_dcm},
|
//{"dcm", test_dcm},
|
||||||
{"omega", test_omega},
|
//{"omega", test_omega},
|
||||||
{"battery", test_battery},
|
{"battery", test_battery},
|
||||||
{"current", test_current},
|
{"current", test_current},
|
||||||
{"relay", test_relay},
|
{"relay", test_relay},
|
||||||
@ -97,7 +99,15 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
|||||||
// ----------------------------------------------------------
|
// ----------------------------------------------------------
|
||||||
read_radio();
|
read_radio();
|
||||||
|
|
||||||
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.rc_1.radio_in, g.rc_2.radio_in, g.rc_3.radio_in, g.rc_4.radio_in, g.rc_5.radio_in, g.rc_6.radio_in, g.rc_7.radio_in, g.rc_8.radio_in);
|
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
|
||||||
|
g.rc_1.radio_in,
|
||||||
|
g.rc_2.radio_in,
|
||||||
|
g.rc_3.radio_in,
|
||||||
|
g.rc_4.radio_in,
|
||||||
|
g.rc_5.radio_in,
|
||||||
|
g.rc_6.radio_in,
|
||||||
|
g.rc_7.radio_in,
|
||||||
|
g.rc_8.radio_in);
|
||||||
|
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
return (0);
|
return (0);
|
||||||
@ -125,7 +135,15 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
|||||||
g.rc_3.calc_pwm();
|
g.rc_3.calc_pwm();
|
||||||
g.rc_4.calc_pwm();
|
g.rc_4.calc_pwm();
|
||||||
|
|
||||||
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (g.rc_1.control_in), (g.rc_2.control_in), (g.rc_3.control_in), (g.rc_4.control_in), g.rc_5.control_in, g.rc_6.control_in, g.rc_7.control_in);
|
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
|
||||||
|
(g.rc_1.control_in),
|
||||||
|
(g.rc_2.control_in),
|
||||||
|
(g.rc_3.control_in),
|
||||||
|
(g.rc_4.control_in),
|
||||||
|
g.rc_5.control_in,
|
||||||
|
g.rc_6.control_in,
|
||||||
|
g.rc_7.control_in);
|
||||||
|
|
||||||
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
|
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
|
||||||
|
|
||||||
/*Serial.printf_P(PSTR( "min: %d"
|
/*Serial.printf_P(PSTR( "min: %d"
|
||||||
@ -439,8 +457,6 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
|||||||
Vector3f accels = imu.get_accel();
|
Vector3f accels = imu.get_accel();
|
||||||
Vector3f gyros = imu.get_gyro();
|
Vector3f gyros = imu.get_gyro();
|
||||||
|
|
||||||
update_trig();
|
|
||||||
|
|
||||||
if(g.compass_enabled){
|
if(g.compass_enabled){
|
||||||
medium_loopCounter++;
|
medium_loopCounter++;
|
||||||
if(medium_loopCounter == 5){
|
if(medium_loopCounter == 5){
|
||||||
@ -452,16 +468,18 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
// We are using the IMU
|
// We are using the IMU
|
||||||
// ---------------------
|
// ---------------------
|
||||||
|
/*
|
||||||
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
|
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
|
||||||
"G: %4.4f, %4.4f, %4.4f\t"),
|
"G: %4.4f, %4.4f, %4.4f\t"),
|
||||||
accels.x, accels.y, accels.z,
|
accels.x, accels.y, accels.z,
|
||||||
gyros.x, gyros.y, gyros.z);
|
gyros.x, gyros.y, gyros.z);
|
||||||
|
*/
|
||||||
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\t"),
|
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\t"),
|
||||||
dcm.roll_sensor,
|
dcm.roll_sensor,
|
||||||
dcm.pitch_sensor,
|
dcm.pitch_sensor,
|
||||||
dcm.yaw_sensor);
|
dcm.yaw_sensor);
|
||||||
|
/*
|
||||||
|
update_trig();
|
||||||
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
|
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
|
||||||
cos_pitch_x,
|
cos_pitch_x,
|
||||||
sin_pitch_y,
|
sin_pitch_y,
|
||||||
@ -469,6 +487,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
|||||||
sin_roll_y,
|
sin_roll_y,
|
||||||
cos_yaw_x, // x
|
cos_yaw_x, // x
|
||||||
sin_yaw_y); // y
|
sin_yaw_y); // y
|
||||||
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
@ -483,43 +502,31 @@ test_gps(uint8_t argc, const Menu::arg *argv)
|
|||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
/*while(1){
|
|
||||||
delay(100);
|
|
||||||
|
|
||||||
update_GPS();
|
|
||||||
|
|
||||||
if(Serial.available() > 0){
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(home.lng != 0){
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}*/
|
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
delay(100);
|
delay(333);
|
||||||
update_GPS();
|
|
||||||
|
|
||||||
calc_distance_error();
|
// Blink GPS LED if we don't have a fix
|
||||||
|
// ------------------------------------
|
||||||
|
update_GPS_light();
|
||||||
|
|
||||||
//if (g_gps->new_data){
|
g_gps->update();
|
||||||
Serial.printf_P(PSTR("Lat: %3.8f, Lon: %3.8f, alt %dm, spd: %d dist:%d, #sats: %d\n"),
|
|
||||||
((float)g_gps->latitude / 10000000),
|
if (g_gps->new_data){
|
||||||
((float)g_gps->longitude / 10000000),
|
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
|
||||||
(int)g_gps->altitude / 100,
|
g_gps->latitude,
|
||||||
(int)g_gps->ground_speed,
|
g_gps->longitude,
|
||||||
(int)wp_distance,
|
g_gps->altitude/100,
|
||||||
(int)g_gps->num_sats);
|
g_gps->num_sats);
|
||||||
//}else{
|
}else{
|
||||||
//Serial.print(".");
|
Serial.print(".");
|
||||||
//}
|
}
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
static int8_t
|
static int8_t
|
||||||
test_dcm(uint8_t argc, const Menu::arg *argv)
|
test_dcm(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
@ -569,8 +576,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
/*
|
/*
|
||||||
static int8_t
|
static int8_t
|
||||||
test_dcm(uint8_t argc, const Menu::arg *argv)
|
test_dcm(uint8_t argc, const Menu::arg *argv)
|
||||||
@ -594,7 +600,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
static int8_t
|
|
||||||
|
/*static int8_t
|
||||||
test_omega(uint8_t argc, const Menu::arg *argv)
|
test_omega(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
static byte ts_num;
|
static byte ts_num;
|
||||||
@ -629,6 +636,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
static int8_t
|
static int8_t
|
||||||
test_battery(uint8_t argc, const Menu::arg *argv)
|
test_battery(uint8_t argc, const Menu::arg *argv)
|
||||||
@ -638,14 +646,11 @@ test_battery(uint8_t argc, const Menu::arg *argv)
|
|||||||
delay(20);
|
delay(20);
|
||||||
read_battery();
|
read_battery();
|
||||||
}
|
}
|
||||||
Serial.printf_P(PSTR("Volts: 1:"));
|
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n")
|
||||||
Serial.print(battery_voltage1, 4);
|
battery_voltage1,
|
||||||
Serial.print(" 2:");
|
battery_voltage2,
|
||||||
Serial.print(battery_voltage2, 4);
|
battery_voltage3,
|
||||||
Serial.print(" 3:");
|
battery_voltage4);
|
||||||
Serial.print(battery_voltage3, 4);
|
|
||||||
Serial.print(" 4:");
|
|
||||||
Serial.println(battery_voltage4, 4);
|
|
||||||
#else
|
#else
|
||||||
Serial.printf_P(PSTR("Not enabled\n"));
|
Serial.printf_P(PSTR("Not enabled\n"));
|
||||||
|
|
||||||
@ -663,7 +668,10 @@ test_current(uint8_t argc, const Menu::arg *argv)
|
|||||||
delay(100);
|
delay(100);
|
||||||
read_radio();
|
read_radio();
|
||||||
read_current();
|
read_current();
|
||||||
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), current_voltage, current_amps, current_total);
|
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
|
||||||
|
current_voltage,
|
||||||
|
current_amps,
|
||||||
|
current_total);
|
||||||
|
|
||||||
//if(g.rc_3.control_in > 0){
|
//if(g.rc_3.control_in > 0){
|
||||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
||||||
@ -678,10 +686,6 @@ test_current(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static int8_t
|
static int8_t
|
||||||
test_relay(uint8_t argc, const Menu::arg *argv)
|
test_relay(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
@ -689,14 +693,14 @@ test_relay(uint8_t argc, const Menu::arg *argv)
|
|||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
Serial.println("Relay on");
|
Serial.printf_P(PSTR("Relay on\n"));
|
||||||
relay_on();
|
relay_on();
|
||||||
delay(3000);
|
delay(3000);
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
|
||||||
Serial.println("Relay off");
|
Serial.printf_P(PSTR("Relay off\n"));
|
||||||
relay_off();
|
relay_off();
|
||||||
delay(3000);
|
delay(3000);
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
@ -725,12 +729,24 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
for(byte i = 0; i <= g.waypoint_total; i++){
|
for(byte i = 0; i <= g.waypoint_total; i++){
|
||||||
struct Location temp = get_wp_with_index(i);
|
struct Location temp = get_wp_with_index(i);
|
||||||
print_waypoint(&temp, i);
|
test_wp_print(&temp, i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
test_wp_print(struct Location *cmd, byte index)
|
||||||
|
{
|
||||||
|
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
|
||||||
|
(int)index,
|
||||||
|
(int)cmd->id,
|
||||||
|
(int)cmd->p1,
|
||||||
|
cmd->alt,
|
||||||
|
cmd->lat,
|
||||||
|
cmd->lng);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
static int8_t
|
static int8_t
|
||||||
test_xbee(uint8_t argc, const Menu::arg *argv)
|
test_xbee(uint8_t argc, const Menu::arg *argv)
|
||||||
@ -772,7 +788,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
home.alt = 0;
|
home.alt = 0;
|
||||||
wp_distance = 0;
|
wp_distance = 0;
|
||||||
init_pressure_ground();
|
init_barometer();
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
if (millis()-fast_loopTimer > 9) {
|
if (millis()-fast_loopTimer > 9) {
|
||||||
@ -828,32 +844,30 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
|||||||
static int8_t
|
static int8_t
|
||||||
test_mag(uint8_t argc, const Menu::arg *argv)
|
test_mag(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
if(g.compass_enabled == false){
|
if(g.compass_enabled) {
|
||||||
Serial.printf_P(PSTR("Compass disabled\n"));
|
|
||||||
return (0);
|
|
||||||
}else{
|
|
||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
delay(250);
|
delay(250);
|
||||||
compass.read();
|
compass.read();
|
||||||
compass.calculate(0,0);
|
compass.calculate(0,0);
|
||||||
Serial.printf_P(PSTR("Heading: ("));
|
Serial.printf_P(PSTR("Heading: %4.2f, XYZ: %4.2f, %4.2f, %4.2f"),
|
||||||
Serial.print(ToDeg(compass.heading));
|
ToDeg(compass.heading),
|
||||||
Serial.printf_P(PSTR(") XYZ: ("));
|
compass.mag_x,
|
||||||
Serial.print(compass.mag_x);
|
compass.mag_y,
|
||||||
Serial.print(comma);
|
compass.mag_z);
|
||||||
Serial.print(compass.mag_y);
|
|
||||||
Serial.print(comma);
|
|
||||||
Serial.print(compass.mag_z);
|
|
||||||
Serial.println(")");
|
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
} else {
|
||||||
|
Serial.printf_P(PSTR("Compass: "));
|
||||||
|
print_enabled(false);
|
||||||
|
return (0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void print_hit_enter()
|
void print_hit_enter()
|
||||||
{
|
{
|
||||||
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
|
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user