mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-19 14:23:57 -04:00
Rover: first cut at porting rover to AP_HAL
This commit is contained in:
Andrew Tridgell
parent
2760c182f5
commit
b284d4c21e
@ -3,8 +3,6 @@
|
||||
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
|
||||
// their default values, place the appropriate #define statements here.
|
||||
|
||||
//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
|
||||
#define LITE DISABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
|
||||
// if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
|
||||
|
||||
|
||||
@ -69,28 +69,19 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
// Header includes
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// AVR runtime
|
||||
#include <avr/io.h>
|
||||
#include <avr/eeprom.h>
|
||||
#include <avr/pgmspace.h>
|
||||
#include <math.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
|
||||
// Libraries
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Progmem.h>
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Menu.h>
|
||||
#include <AP_Param.h>
|
||||
#include <Arduino_Mega_ISR_Registry.h>
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
#include <AP_GPS.h> // ArduPilot GPS library
|
||||
#include <I2C.h> // Wayne Truchsess I2C lib
|
||||
#include <SPI.h> // Arduino SPI lib
|
||||
#include <AP_Semaphore.h> // for removing conflict between optical flow and dataflash on SPI3 bus
|
||||
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter
|
||||
#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
|
||||
#include <AP_ADC_AnalogSource.h>
|
||||
#include <AP_Baro.h>
|
||||
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
@ -108,6 +99,14 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
#include <AP_Airspeed.h> // needed for AHRS build
|
||||
#include <memcheck.h>
|
||||
#include <DataFlash.h>
|
||||
#include <SITL.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include <AP_HAL_AVR.h>
|
||||
#include <AP_HAL_AVR_SITL.h>
|
||||
#include <AP_HAL_Empty.h>
|
||||
#include "compat.h"
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
@ -119,24 +118,9 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Serial ports
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Note that FastSerial port buffers are allocated at ::begin time,
|
||||
// so there is not much of a penalty to defining ports that we don't
|
||||
// use.
|
||||
//
|
||||
FastSerialPort0(Serial); // FTDI/console
|
||||
FastSerialPort1(Serial1); // GPS port
|
||||
#if TELEMETRY_UART2 == ENABLED
|
||||
// solder bridge set to enable UART2 instead of USB MUX
|
||||
FastSerialPort2(Serial3);
|
||||
#else
|
||||
FastSerialPort3(Serial3); // Telemetry port for APM1
|
||||
#endif
|
||||
AP_HAL::BetterStream* cliSerial;
|
||||
|
||||
static FastSerial *cliSerial = &Serial;
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
// this sets up the parameter table, and sets the default values. This
|
||||
// must be the first AP_Param variable declared to ensure its
|
||||
@ -144,31 +128,6 @@ static FastSerial *cliSerial = &Serial;
|
||||
// variables
|
||||
AP_Param param_loader(var_info, WP_START_BYTE);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// ISR Registry
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
Arduino_Mega_ISR_Registry isr_registry;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// APM_RC_Class Instance
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
APM_RC_APM2 APM_RC;
|
||||
#else
|
||||
APM_RC_APM1 APM_RC;
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Dataflash
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
AP_Semaphore spi3_semaphore;
|
||||
DataFlash_APM2 DataFlash(&spi3_semaphore);
|
||||
#else
|
||||
DataFlash_APM1 DataFlash;
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// the rate we run the main loop at
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -187,6 +146,17 @@ static Parameters g;
|
||||
// prototypes
|
||||
static void update_events(void);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// DataFlash
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
DataFlash_APM1 DataFlash;
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||
DataFlash_APM2 DataFlash;
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
DataFlash_SITL DataFlash;
|
||||
#endif
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Sensors
|
||||
@ -214,36 +184,34 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||
static AP_ADC_ADS7844 adc;
|
||||
#endif
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
AP_Baro_BMP085_HIL barometer;
|
||||
AP_Compass_HIL compass;
|
||||
#include <SITL.h>
|
||||
SITL sitl;
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
static AP_Compass_HIL compass;
|
||||
static SITL sitl;
|
||||
#else
|
||||
static AP_Compass_HMC5843 compass;
|
||||
#endif
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
||||
AP_GPS_Auto g_gps_driver(&g_gps);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
||||
AP_GPS_NMEA g_gps_driver(&Serial1);
|
||||
AP_GPS_NMEA g_gps_driver();
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
||||
AP_GPS_SIRF g_gps_driver(&Serial1);
|
||||
AP_GPS_SIRF g_gps_driver();
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
||||
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
||||
AP_GPS_UBLOX g_gps_driver();
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
||||
AP_GPS_MTK g_gps_driver(&Serial1);
|
||||
AP_GPS_MTK g_gps_driver();
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
||||
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
||||
AP_GPS_MTK16 g_gps_driver();
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
||||
AP_GPS_None g_gps_driver(NULL);
|
||||
AP_GPS_None g_gps_driver();
|
||||
|
||||
#else
|
||||
#error Unrecognised GPS_PROTOCOL setting.
|
||||
@ -251,7 +219,9 @@ AP_GPS_None g_gps_driver(NULL);
|
||||
|
||||
# if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
|
||||
AP_InertialSensor_MPU6000 ins;
|
||||
# else
|
||||
# elif CONFIG_INS_TYPE == CONFIG_INS_SITL
|
||||
AP_InertialSensor_Stub ins;
|
||||
#else
|
||||
AP_InertialSensor_Oilpan ins( &adc );
|
||||
#endif // CONFIG_INS_TYPE
|
||||
|
||||
@ -274,9 +244,6 @@ AP_Compass_HIL compass; // never used
|
||||
#error Unrecognised HIL_MODE setting.
|
||||
#endif // HIL MODE
|
||||
|
||||
// we always have a timer scheduler
|
||||
AP_TimerProcess timer_scheduler;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// GCS selection
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -284,22 +251,23 @@ AP_TimerProcess timer_scheduler;
|
||||
GCS_MAVLINK gcs0;
|
||||
GCS_MAVLINK gcs3;
|
||||
|
||||
// a pin for reading the receiver RSSI voltage. The scaling by 0.25
|
||||
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
|
||||
AP_HAL::AnalogSource *rssi_analog_source;
|
||||
|
||||
AP_HAL::AnalogSource *vcc_pin;
|
||||
|
||||
AP_HAL::AnalogSource * batt_volt_pin;
|
||||
AP_HAL::AnalogSource * batt_curr_pin;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// SONAR selection
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
ModeFilterInt16_Size5 sonar_mode_filter(2);
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
/*
|
||||
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
|
||||
AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
|
||||
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
|
||||
AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
|
||||
#endif
|
||||
AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
|
||||
*/
|
||||
AP_AnalogSource_Arduino sonar_analog_source(A0); // use AN0 analog pin for APM2 on left
|
||||
AP_RangeFinder_SharpGP2Y sonar(&sonar_analog_source, &sonar_mode_filter);
|
||||
AP_HAL::AnalogSource *sonar_analog_source;
|
||||
AP_RangeFinder_MaxsonarXL *sonar;
|
||||
#endif
|
||||
|
||||
// relay support
|
||||
@ -341,10 +309,10 @@ static const char *comma = ",";
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// This is the state of the flight control system
|
||||
// There are multiple states defined such as MANUAL, FBW-A, AUTO
|
||||
byte control_mode = INITIALISING;
|
||||
uint8_t control_mode = INITIALISING;
|
||||
// Used to maintain the state of the previous control switch position
|
||||
// This is set to -1 when we need to re-read the switch
|
||||
byte oldSwitchPosition;
|
||||
uint8_t oldSwitchPosition;
|
||||
// These are values received from the GCS if the user is using GCS joystick
|
||||
// control and are substituted for the values coming from the RC radio
|
||||
static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
|
||||
@ -383,7 +351,7 @@ static const float t7 = 10000000.0;
|
||||
|
||||
// A counter used to count down valid gps fixes to allow the gps estimate to settle
|
||||
// before recording our home position (and executing a ground start if we booted with an air start)
|
||||
static byte ground_start_count = 5;
|
||||
static uint8_t ground_start_count = 5;
|
||||
// Used to compute a speed estimate from the first valid gps fixes to decide if we are
|
||||
// on the ground or in the air. Used to decide if a ground start is appropriate if we
|
||||
// booted with an air start.
|
||||
@ -416,12 +384,12 @@ static bool rtl_complete = false;
|
||||
|
||||
// There may be two active commands in Auto mode.
|
||||
// This indicates the active navigation command by index number
|
||||
static byte nav_command_index;
|
||||
static uint8_t nav_command_index;
|
||||
// This indicates the active non-navigation command by index number
|
||||
static byte non_nav_command_index;
|
||||
static uint8_t non_nav_command_index;
|
||||
// This is the command type (eg navigate to waypoint) of the active navigation command
|
||||
static byte nav_command_ID = NO_COMMAND;
|
||||
static byte non_nav_command_ID = NO_COMMAND;
|
||||
static uint8_t nav_command_ID = NO_COMMAND;
|
||||
static uint8_t non_nav_command_ID = NO_COMMAND;
|
||||
|
||||
static float groundspeed_error;
|
||||
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
||||
@ -455,7 +423,7 @@ static float crosstrack_error;
|
||||
// Used to track the CH7 toggle state.
|
||||
// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
|
||||
// This allows advanced functionality to know when to execute
|
||||
static boolean trim_flag;
|
||||
static bool trim_flag;
|
||||
// This register tracks the current Mission Command index when writing
|
||||
// a mission using CH7 in flight
|
||||
static int8_t CH7_wp_index;
|
||||
@ -500,7 +468,7 @@ static int32_t wp_totalDistance;
|
||||
// repeating event control
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Flag indicating current event type
|
||||
static byte event_id;
|
||||
static uint8_t event_id;
|
||||
// when the event was started in ms
|
||||
static int32_t event_timer;
|
||||
// how long to delay the next firing of event in millis
|
||||
@ -582,16 +550,16 @@ static uint16_t mainLoop_count;
|
||||
// Time in miliseconds of start of medium control loop. Milliseconds
|
||||
static uint32_t medium_loopTimer;
|
||||
// Counters for branching from main control loop to slower loops
|
||||
static byte medium_loopCounter;
|
||||
static uint8_t medium_loopCounter;
|
||||
// Number of milliseconds used in last medium loop cycle
|
||||
static uint8_t delta_ms_medium_loop;
|
||||
|
||||
// Counters for branching from medium control loop to slower loops
|
||||
static byte slow_loopCounter;
|
||||
static uint8_t slow_loopCounter;
|
||||
// Counter to trigger execution of very low rate processes
|
||||
static byte superslow_loopCounter;
|
||||
static uint8_t superslow_loopCounter;
|
||||
// Counter to trigger execution of 1 Hz processes
|
||||
static byte counter_one_herz;
|
||||
static uint8_t counter_one_herz;
|
||||
|
||||
// % MCU cycles used
|
||||
static float load;
|
||||
@ -602,6 +570,27 @@ static float load;
|
||||
|
||||
void setup() {
|
||||
memcheck_init();
|
||||
cliSerial = hal.console;
|
||||
|
||||
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 0.25);
|
||||
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
|
||||
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
|
||||
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
|
||||
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
|
||||
sonar_analog_source = new AP_ADC_AnalogSource(
|
||||
&adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
|
||||
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
|
||||
sonar_analog_source = hal.analogin->channel(
|
||||
CONFIG_SONAR_SOURCE_ANALOG_PIN);
|
||||
#else
|
||||
#warning "Invalid CONFIG_SONAR_SOURCE"
|
||||
#endif
|
||||
sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
|
||||
&sonar_mode_filter);
|
||||
#endif
|
||||
|
||||
init_ardupilot();
|
||||
}
|
||||
|
||||
@ -688,7 +677,7 @@ static void fast_loop()
|
||||
// ----------
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
if(g.sonar_enabled){
|
||||
sonar_dist = sonar.read();
|
||||
sonar_dist = sonar->read();
|
||||
|
||||
if(sonar_dist <= g.sonar_trigger) { // obstacle detected in front
|
||||
obstacle = true;
|
||||
@ -972,3 +961,5 @@ static void update_navigation()
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
@ -120,10 +120,10 @@ static void set_servos(void)
|
||||
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
||||
// send values to the PWM timers for output
|
||||
// ----------------------------------------
|
||||
APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
|
||||
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
|
||||
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
|
||||
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos
|
||||
// Route configurable aux. functions to their respective servos
|
||||
|
||||
g.rc_5.output_ch(CH_5);
|
||||
@ -134,18 +134,22 @@ static void set_servos(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
static void demo_servos(byte i) {
|
||||
static bool demoing_servos;
|
||||
|
||||
while(i > 0){
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
|
||||
static void demo_servos(uint8_t i) {
|
||||
|
||||
while(i > 0) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
|
||||
demoing_servos = true;
|
||||
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
||||
APM_RC.OutputCh(1, 1400);
|
||||
mavlink_delay(400);
|
||||
APM_RC.OutputCh(1, 1600);
|
||||
mavlink_delay(200);
|
||||
APM_RC.OutputCh(1, 1500);
|
||||
hal.rcout->write(1, 1400);
|
||||
mavlink_delay(400);
|
||||
hal.rcout->write(1, 1600);
|
||||
mavlink_delay(200);
|
||||
hal.rcout->write(1, 1500);
|
||||
#endif
|
||||
mavlink_delay(400);
|
||||
i--;
|
||||
}
|
||||
demoing_servos = false;
|
||||
mavlink_delay(400);
|
||||
i--;
|
||||
}
|
||||
}
|
||||
|
||||
@ -6,10 +6,8 @@
|
||||
#ifndef __GCS_H
|
||||
#define __GCS_H
|
||||
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
#include <GPS.h>
|
||||
#include <Stream.h>
|
||||
#include <stdint.h>
|
||||
|
||||
///
|
||||
@ -39,7 +37,7 @@ public:
|
||||
///
|
||||
/// @param port The stream over which messages are exchanged.
|
||||
///
|
||||
void init(FastSerial *port) {
|
||||
void init(AP_HAL::UARTDriver *port) {
|
||||
_port = port;
|
||||
initialised = true;
|
||||
last_gps_satellites = 255;
|
||||
@ -61,19 +59,12 @@ public:
|
||||
///
|
||||
void send_message(enum ap_message id) {}
|
||||
|
||||
/// Send a text message.
|
||||
///
|
||||
/// @param severity A value describing the importance of the message.
|
||||
/// @param str The text to be sent.
|
||||
///
|
||||
void send_text(gcs_severity severity, const char *str) {}
|
||||
|
||||
/// Send a text message with a PSTR()
|
||||
///
|
||||
/// @param severity A value describing the importance of the message.
|
||||
/// @param str The text to be sent.
|
||||
///
|
||||
void send_text(gcs_severity severity, const prog_char_t *str) {}
|
||||
void send_text_P(gcs_severity severity, const prog_char_t *str) {}
|
||||
|
||||
// send streams which match frequency range
|
||||
void data_stream_send(void);
|
||||
@ -86,7 +77,7 @@ public:
|
||||
|
||||
protected:
|
||||
/// The stream we are communicating over
|
||||
FastSerial *_port;
|
||||
AP_HAL::UARTDriver * _port;
|
||||
};
|
||||
|
||||
//
|
||||
@ -105,10 +96,9 @@ class GCS_MAVLINK : public GCS_Class
|
||||
public:
|
||||
GCS_MAVLINK();
|
||||
void update(void);
|
||||
void init(FastSerial *port);
|
||||
void init(AP_HAL::UARTDriver *port);
|
||||
void send_message(enum ap_message id);
|
||||
void send_text(gcs_severity severity, const char *str);
|
||||
void send_text(gcs_severity severity, const prog_char_t *str);
|
||||
void send_text_P(gcs_severity severity, const prog_char_t *str);
|
||||
void data_stream_send(void);
|
||||
void queued_param_send();
|
||||
void queued_waypoint_send();
|
||||
@ -132,6 +122,10 @@ public:
|
||||
// see if we should send a stream now. Called at 50Hz
|
||||
bool stream_trigger(enum streams stream_num);
|
||||
|
||||
// this costs us 51 bytes per instance, but means that low priority
|
||||
// messages don't block the CPU
|
||||
mavlink_statustext_t pending_status;
|
||||
|
||||
private:
|
||||
void handleMessage(mavlink_message_t * msg);
|
||||
|
||||
|
||||
@ -3,10 +3,6 @@
|
||||
// use this to prevent recursion during sensor init
|
||||
static bool in_mavlink_delay;
|
||||
|
||||
// this costs us 51 bytes, but means that low priority
|
||||
// messages don't block the CPU
|
||||
static mavlink_statustext_t pending_status;
|
||||
|
||||
// true when we have received at least 1 MAVLink packet
|
||||
static bool mavlink_active;
|
||||
|
||||
@ -192,8 +188,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
||||
|
||||
static void NOINLINE send_meminfo(mavlink_channel_t chan)
|
||||
{
|
||||
#if CONFIG_HAL_BOARD != HAL_BOARD_AVR_SITL
|
||||
extern unsigned __brkval;
|
||||
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
||||
#endif
|
||||
}
|
||||
|
||||
static void NOINLINE send_location(mavlink_channel_t chan)
|
||||
@ -308,18 +306,34 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
||||
|
||||
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_servo_output_raw_send(
|
||||
chan,
|
||||
micros(),
|
||||
0, // port
|
||||
APM_RC.OutputCh_current(0),
|
||||
APM_RC.OutputCh_current(1),
|
||||
APM_RC.OutputCh_current(2),
|
||||
APM_RC.OutputCh_current(3),
|
||||
APM_RC.OutputCh_current(4),
|
||||
APM_RC.OutputCh_current(5),
|
||||
APM_RC.OutputCh_current(6),
|
||||
APM_RC.OutputCh_current(7));
|
||||
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
||||
mavlink_msg_servo_output_raw_send(
|
||||
chan,
|
||||
micros(),
|
||||
0, // port
|
||||
hal.rcout->read(0),
|
||||
hal.rcout->read(1),
|
||||
hal.rcout->read(2),
|
||||
hal.rcout->read(3),
|
||||
hal.rcout->read(4),
|
||||
hal.rcout->read(5),
|
||||
hal.rcout->read(6),
|
||||
hal.rcout->read(7));
|
||||
#else
|
||||
extern RC_Channel* rc_ch[8];
|
||||
mavlink_msg_servo_output_raw_send(
|
||||
chan,
|
||||
micros(),
|
||||
0, // port
|
||||
rc_ch[0]->radio_out,
|
||||
rc_ch[1]->radio_out,
|
||||
rc_ch[2]->radio_out,
|
||||
rc_ch[3]->radio_out,
|
||||
rc_ch[4]->radio_out,
|
||||
rc_ch[5]->radio_out,
|
||||
rc_ch[6]->radio_out,
|
||||
rc_ch[7]->radio_out);
|
||||
#endif
|
||||
}
|
||||
|
||||
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
|
||||
@ -390,7 +404,7 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan)
|
||||
|
||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
// report simulator state
|
||||
static void NOINLINE send_simstate(mavlink_channel_t chan)
|
||||
{
|
||||
@ -398,15 +412,13 @@ static void NOINLINE send_simstate(mavlink_channel_t chan)
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef DESKTOP_BUILD
|
||||
static void NOINLINE send_hwstatus(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_hwstatus_send(
|
||||
chan,
|
||||
board_voltage(),
|
||||
I2c.lockup_count());
|
||||
hal.i2c->lockup_count());
|
||||
}
|
||||
#endif
|
||||
|
||||
static void NOINLINE send_gps_status(mavlink_channel_t chan)
|
||||
{
|
||||
@ -429,10 +441,11 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
|
||||
|
||||
static void NOINLINE send_statustext(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
|
||||
mavlink_msg_statustext_send(
|
||||
chan,
|
||||
pending_status.severity,
|
||||
pending_status.text);
|
||||
s->severity,
|
||||
s->text);
|
||||
}
|
||||
|
||||
// are we still delaying telemetry to try to avoid Xbee bricking?
|
||||
@ -581,17 +594,15 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
|
||||
break;
|
||||
|
||||
case MSG_SIMSTATE:
|
||||
#ifdef DESKTOP_BUILD
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
CHECK_PAYLOAD_SIZE(SIMSTATE);
|
||||
send_simstate(chan);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MSG_HWSTATUS:
|
||||
#ifndef DESKTOP_BUILD
|
||||
CHECK_PAYLOAD_SIZE(HWSTATUS);
|
||||
send_hwstatus(chan);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MSG_RETRY_DEFERRED:
|
||||
@ -668,8 +679,9 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
|
||||
|
||||
if (severity == SEVERITY_LOW) {
|
||||
// send via the deferred queuing system
|
||||
pending_status.severity = (uint8_t)severity;
|
||||
strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
|
||||
mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
|
||||
s->severity = (uint8_t)severity;
|
||||
strncpy((char *)s->text, str, sizeof(s->text));
|
||||
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
|
||||
} else {
|
||||
// send immediately
|
||||
@ -699,17 +711,17 @@ GCS_MAVLINK::GCS_MAVLINK() :
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::init(FastSerial * port)
|
||||
GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
|
||||
{
|
||||
GCS_Class::init(port);
|
||||
if (port == &Serial) {
|
||||
if (port == (AP_HAL::BetterStream*)hal.uartA) {
|
||||
mavlink_comm_0_port = port;
|
||||
chan = MAVLINK_COMM_0;
|
||||
}else{
|
||||
mavlink_comm_1_port = port;
|
||||
chan = MAVLINK_COMM_1;
|
||||
}
|
||||
_queued_parameter = NULL;
|
||||
_queued_parameter = NULL;
|
||||
}
|
||||
|
||||
void
|
||||
@ -881,13 +893,7 @@ GCS_MAVLINK::send_message(enum ap_message id)
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
|
||||
{
|
||||
mavlink_send_text(chan,severity,str);
|
||||
}
|
||||
|
||||
void
|
||||
GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
|
||||
GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
|
||||
{
|
||||
mavlink_statustext_t m;
|
||||
uint8_t i;
|
||||
@ -987,7 +993,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
uint8_t result;
|
||||
|
||||
// do command
|
||||
send_text(SEVERITY_LOW,PSTR("command received: "));
|
||||
send_text_P(SEVERITY_LOW,PSTR("command received: "));
|
||||
|
||||
switch(packet.command) {
|
||||
|
||||
@ -1274,7 +1280,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
if (packet.start_index > g.command_total ||
|
||||
packet.end_index > g.command_total ||
|
||||
packet.end_index < packet.start_index) {
|
||||
send_text(SEVERITY_LOW,PSTR("flight plan update rejected"));
|
||||
send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));
|
||||
break;
|
||||
}
|
||||
|
||||
@ -1457,7 +1463,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
msg->compid,
|
||||
result);
|
||||
|
||||
send_text(SEVERITY_LOW,PSTR("flight plan received"));
|
||||
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
|
||||
waypoint_receiving = false;
|
||||
// XXX ignores waypoint radius for individual waypoints, can
|
||||
// only set WP_RADIUS parameter
|
||||
@ -1545,30 +1551,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
} // end case
|
||||
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
|
||||
{
|
||||
// allow override of RC channel values for HIL
|
||||
// or for complete GCS control of switch position
|
||||
// and RC PWM values.
|
||||
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
|
||||
mavlink_rc_channels_override_t packet;
|
||||
int16_t v[8];
|
||||
mavlink_msg_rc_channels_override_decode(msg, &packet);
|
||||
{
|
||||
// allow override of RC channel values for HIL
|
||||
// or for complete GCS control of switch position
|
||||
// and RC PWM values.
|
||||
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
|
||||
mavlink_rc_channels_override_t packet;
|
||||
int16_t v[8];
|
||||
mavlink_msg_rc_channels_override_decode(msg, &packet);
|
||||
|
||||
if (mavlink_check_target(packet.target_system,packet.target_component))
|
||||
break;
|
||||
|
||||
v[0] = packet.chan1_raw;
|
||||
v[1] = packet.chan2_raw;
|
||||
v[2] = packet.chan3_raw;
|
||||
v[3] = packet.chan4_raw;
|
||||
v[4] = packet.chan5_raw;
|
||||
v[5] = packet.chan6_raw;
|
||||
v[6] = packet.chan7_raw;
|
||||
v[7] = packet.chan8_raw;
|
||||
rc_override_active = APM_RC.setHIL(v);
|
||||
rc_override_fs_timer = millis();
|
||||
if (mavlink_check_target(packet.target_system,packet.target_component))
|
||||
break;
|
||||
}
|
||||
|
||||
v[0] = packet.chan1_raw;
|
||||
v[1] = packet.chan2_raw;
|
||||
v[2] = packet.chan3_raw;
|
||||
v[3] = packet.chan4_raw;
|
||||
v[4] = packet.chan5_raw;
|
||||
v[5] = packet.chan6_raw;
|
||||
v[6] = packet.chan7_raw;
|
||||
v[7] = packet.chan8_raw;
|
||||
|
||||
hal.rcin->set_overrides(v, 8);
|
||||
|
||||
rc_override_fs_timer = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
{
|
||||
@ -1811,42 +1819,36 @@ GCS_MAVLINK::queued_waypoint_send()
|
||||
}
|
||||
|
||||
/*
|
||||
a delay() callback that processes MAVLink packets. We set this as the
|
||||
callback in long running library initialisation routines to allow
|
||||
MAVLink to process packets while waiting for the initialisation to
|
||||
complete
|
||||
*/
|
||||
static void mavlink_delay(unsigned long t)
|
||||
* a delay() callback that processes MAVLink packets. We set this as the
|
||||
* callback in long running library initialisation routines to allow
|
||||
* MAVLink to process packets while waiting for the initialisation to
|
||||
* complete
|
||||
*/
|
||||
static void mavlink_delay_cb()
|
||||
{
|
||||
uint32_t tstart;
|
||||
static uint32_t last_1hz, last_50hz;
|
||||
|
||||
if (in_mavlink_delay) {
|
||||
// this should never happen, but let's not tempt fate by
|
||||
// letting the stack grow too much
|
||||
delay(t);
|
||||
return;
|
||||
}
|
||||
static uint32_t last_1hz, last_50hz, last_5s;
|
||||
if (!gcs0.initialised) return;
|
||||
|
||||
in_mavlink_delay = true;
|
||||
|
||||
tstart = millis();
|
||||
do {
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
last_1hz = tnow;
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
gcs_send_message(MSG_EXTENDED_STATUS1);
|
||||
}
|
||||
if (tnow - last_50hz > 20) {
|
||||
last_50hz = tnow;
|
||||
gcs_update();
|
||||
}
|
||||
delay(1);
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
last_1hz = tnow;
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
gcs_send_message(MSG_EXTENDED_STATUS1);
|
||||
}
|
||||
if (tnow - last_50hz > 20) {
|
||||
last_50hz = tnow;
|
||||
gcs_update();
|
||||
gcs_data_stream_send();
|
||||
}
|
||||
if (tnow - last_5s > 5000) {
|
||||
last_5s = tnow;
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
|
||||
}
|
||||
#if USB_MUX_PIN > 0
|
||||
check_usb_mux();
|
||||
check_usb_mux();
|
||||
#endif
|
||||
} while (millis() - tstart < t);
|
||||
|
||||
in_mavlink_delay = false;
|
||||
}
|
||||
@ -1886,31 +1888,32 @@ static void gcs_update(void)
|
||||
|
||||
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
||||
{
|
||||
gcs0.send_text(severity, str);
|
||||
gcs0.send_text_P(severity, str);
|
||||
if (gcs3.initialised) {
|
||||
gcs3.send_text(severity, str);
|
||||
gcs3.send_text_P(severity, str);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
send a low priority formatted message to the GCS
|
||||
only one fits in the queue, so if you send more than one before the
|
||||
last one gets into the serial buffer then the old one will be lost
|
||||
* send a low priority formatted message to the GCS
|
||||
* only one fits in the queue, so if you send more than one before the
|
||||
* last one gets into the serial buffer then the old one will be lost
|
||||
*/
|
||||
static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
|
||||
void gcs_send_text_fmt(const prog_char_t *fmt, ...)
|
||||
{
|
||||
char fmtstr[40];
|
||||
va_list ap;
|
||||
va_list arg_list;
|
||||
uint8_t i;
|
||||
for (i=0; i<sizeof(fmtstr)-1; i++) {
|
||||
fmtstr[i] = pgm_read_byte((const prog_char *)(fmt++));
|
||||
if (fmtstr[i] == 0) break;
|
||||
}
|
||||
fmtstr[i] = 0;
|
||||
pending_status.severity = (uint8_t)SEVERITY_LOW;
|
||||
va_start(ap, fmt);
|
||||
vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
|
||||
va_end(ap);
|
||||
gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
|
||||
va_start(arg_list, fmt);
|
||||
vsnprintf((char *)gcs0.pending_status.text, sizeof(gcs0.pending_status.text), fmtstr, arg_list);
|
||||
va_end(arg_list);
|
||||
gcs3.pending_status = gcs0.pending_status;
|
||||
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
|
||||
if (gcs3.initialised) {
|
||||
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
|
||||
|
||||
@ -108,7 +108,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
int16_t dump_log;
|
||||
int16_t dump_log_start;
|
||||
int16_t dump_log_end;
|
||||
byte last_log_num;
|
||||
uint8_t last_log_num;
|
||||
|
||||
// check that the requested log number can be read
|
||||
dump_log = argv[1].i;
|
||||
@ -153,7 +153,7 @@ void erase_callback(unsigned long t) {
|
||||
static void do_erase_logs(void)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("\nErasing log...\n"));
|
||||
DataFlash.EraseAll(erase_callback);
|
||||
DataFlash.EraseAll();
|
||||
cliSerial->printf_P(PSTR("\nLog erased.\n"));
|
||||
}
|
||||
|
||||
@ -257,7 +257,7 @@ static void Log_Write_Performance()
|
||||
|
||||
// Write a command processing packet. Total length : 19 bytes
|
||||
//void Log_Write_Cmd(byte num, byte id, byte p1, int32_t alt, int32_t lat, int32_t lng)
|
||||
static void Log_Write_Cmd(byte num, struct Location *wp)
|
||||
static void Log_Write_Cmd(uint8_t num, struct Location *wp)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
@ -271,7 +271,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp)
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
static void Log_Write_Startup(byte type)
|
||||
static void Log_Write_Startup(uint8_t type)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
@ -330,7 +330,7 @@ static void Log_Write_Nav_Tuning()
|
||||
}
|
||||
|
||||
// Write a mode packet. Total length : 5 bytes
|
||||
static void Log_Write_Mode(byte mode)
|
||||
static void Log_Write_Mode(uint8_t mode)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
@ -341,7 +341,7 @@ static void Log_Write_Mode(byte mode)
|
||||
|
||||
// Write an GPS packet. Total length : 30 bytes
|
||||
static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
|
||||
int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats)
|
||||
int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
@ -468,7 +468,7 @@ static void Log_Read_Performance()
|
||||
// Read a command processing packet
|
||||
static void Log_Read_Cmd()
|
||||
{
|
||||
byte logvarb;
|
||||
uint8_t logvarb;
|
||||
int32_t logvarl;
|
||||
|
||||
cliSerial->printf_P(PSTR("CMD: "));
|
||||
@ -489,7 +489,7 @@ static void Log_Read_Cmd()
|
||||
|
||||
static void Log_Read_Startup()
|
||||
{
|
||||
byte logbyte = DataFlash.ReadByte();
|
||||
uint8_t logbyte = DataFlash.ReadByte();
|
||||
|
||||
if (logbyte == TYPE_AIRSTART_MSG)
|
||||
cliSerial->printf_P(PSTR("AIR START - "));
|
||||
@ -524,7 +524,7 @@ static void Log_Read_Mode()
|
||||
static void Log_Read_GPS()
|
||||
{
|
||||
int32_t l[7];
|
||||
byte b[2];
|
||||
uint8_t b[2];
|
||||
int16_t j,k,m;
|
||||
l[0] = DataFlash.ReadLong();
|
||||
b[0] = DataFlash.ReadByte();
|
||||
@ -595,8 +595,8 @@ static void Log_Read(int16_t start_page, int16_t end_page)
|
||||
// Read the DataFlash log memory : Packet Parser
|
||||
static int Log_Read_Process(int16_t start_page, int16_t end_page)
|
||||
{
|
||||
byte data;
|
||||
byte log_step = 0;
|
||||
uint8_t data;
|
||||
uint8_t log_step = 0;
|
||||
int page = start_page;
|
||||
int packet_count = 0;
|
||||
|
||||
@ -679,13 +679,13 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page)
|
||||
#else // LOGGING_ENABLED
|
||||
|
||||
// dummy functions
|
||||
static void Log_Write_Mode(byte mode) {}
|
||||
static void Log_Write_Startup(byte type) {}
|
||||
static void Log_Write_Cmd(byte num, struct Location *wp) {}
|
||||
static void Log_Write_Mode(uint8_t mode) {}
|
||||
static void Log_Write_Startup(uint8_t type) {}
|
||||
static void Log_Write_Cmd(uint8_t num, struct Location *wp) {}
|
||||
static void Log_Write_Current() {}
|
||||
static void Log_Write_Nav_Tuning() {}
|
||||
static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
|
||||
int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {}
|
||||
int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) {}
|
||||
static void Log_Write_Performance() {}
|
||||
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
||||
static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {}
|
||||
|
||||
@ -18,15 +18,9 @@ hilnocli:
|
||||
heli:
|
||||
make -f Makefile EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
|
||||
|
||||
apm2:
|
||||
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
|
||||
|
||||
apm2beta:
|
||||
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
|
||||
|
||||
sitl:
|
||||
make -f ../libraries/Desktop/Makefile.desktop
|
||||
|
||||
etags:
|
||||
cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)
|
||||
|
||||
|
||||
@ -50,6 +50,9 @@ public:
|
||||
k_param_reset_switch_chan,
|
||||
k_param_manual_level,
|
||||
k_param_ins, // libraries/AP_InertialSensor variables
|
||||
k_param_rssi_pin,
|
||||
k_param_battery_volt_pin,
|
||||
k_param_battery_curr_pin,
|
||||
|
||||
|
||||
// 110: Telemetry control
|
||||
@ -312,7 +315,11 @@ public:
|
||||
AP_Float volt_div_ratio;
|
||||
AP_Float curr_amp_per_volt;
|
||||
AP_Float input_voltage;
|
||||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int8 rssi_pin;
|
||||
AP_Int8 battery_volt_pin;
|
||||
AP_Int8 battery_curr_pin;
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
AP_Int8 sonar_enabled;
|
||||
|
||||
@ -201,6 +201,28 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
|
||||
GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE),
|
||||
GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE),
|
||||
|
||||
// @Param: BATT_VOLT_PIN
|
||||
// @DisplayName: Battery Voltage sensing pin
|
||||
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13.
|
||||
// @Values: -1:Disabled, 0:A0, 1:A1, 13:A13
|
||||
// @User: Standard
|
||||
GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", 1),
|
||||
|
||||
// @Param: BATT_CURR_PIN
|
||||
// @DisplayName: Battery Current sensing pin
|
||||
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13.
|
||||
// @Values: -1:Disabled, 1:A1, 2:A2, 12:A12
|
||||
// @User: Standard
|
||||
GSCALAR(battery_curr_pin, "BATT_CURR_PIN", 2),
|
||||
|
||||
// @Param: RSSI_PIN
|
||||
// @DisplayName: Receiver RSSI sensing pin
|
||||
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
|
||||
// @Values: -1:Disabled, 0:A0, 1:A1, 13:A13
|
||||
// @User: Standard
|
||||
GSCALAR(rssi_pin, "RSSI_PIN", -1),
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
// @Param: SONAR_ENABLE
|
||||
@ -250,7 +272,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
GOBJECT(ins, "INS_", AP_InertialSensor),
|
||||
#endif
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
// @Group: SIM_
|
||||
// @Path: ../libraries/SITL/SITL.cpp
|
||||
GOBJECT(sitl, "SIM_", SITL),
|
||||
|
||||
@ -27,7 +27,7 @@ static void init_commands()
|
||||
static struct Location get_cmd_with_index(int i)
|
||||
{
|
||||
struct Location temp;
|
||||
long mem;
|
||||
uint16_t mem;
|
||||
|
||||
// Find out proper location in memory by using the start_byte position + the index
|
||||
// --------------------------------------------------------------------------------
|
||||
@ -37,22 +37,22 @@ static struct Location get_cmd_with_index(int i)
|
||||
}else{
|
||||
// read WP position
|
||||
mem = (WP_START_BYTE) + (i * WP_SIZE);
|
||||
temp.id = eeprom_read_byte((uint8_t*)mem);
|
||||
temp.id = hal.storage->read_byte(mem);
|
||||
|
||||
mem++;
|
||||
temp.options = eeprom_read_byte((uint8_t*)mem);
|
||||
temp.options = hal.storage->read_byte(mem);
|
||||
|
||||
mem++;
|
||||
temp.p1 = eeprom_read_byte((uint8_t*)mem);
|
||||
temp.p1 = hal.storage->read_byte(mem);
|
||||
|
||||
mem++;
|
||||
temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
|
||||
temp.alt = (long)hal.storage->read_dword(mem);
|
||||
|
||||
mem += 4;
|
||||
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
|
||||
temp.lat = (long)hal.storage->read_dword(mem);
|
||||
|
||||
mem += 4;
|
||||
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
|
||||
temp.lng = (long)hal.storage->read_dword(mem);
|
||||
}
|
||||
|
||||
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
|
||||
@ -68,7 +68,7 @@ static struct Location get_cmd_with_index(int i)
|
||||
static void set_cmd_with_index(struct Location temp, int i)
|
||||
{
|
||||
i = constrain(i, 0, g.command_total.get());
|
||||
intptr_t mem = WP_START_BYTE + (i * WP_SIZE);
|
||||
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
|
||||
|
||||
// Set altitude options bitmask
|
||||
// XXX What is this trying to do?
|
||||
@ -78,22 +78,22 @@ static void set_cmd_with_index(struct Location temp, int i)
|
||||
temp.options = 0;
|
||||
}
|
||||
|
||||
eeprom_write_byte((uint8_t *) mem, temp.id);
|
||||
hal.storage->write_byte(mem, temp.id);
|
||||
|
||||
mem++;
|
||||
eeprom_write_byte((uint8_t *) mem, temp.options);
|
||||
mem++;
|
||||
hal.storage->write_byte(mem, temp.options);
|
||||
|
||||
mem++;
|
||||
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
||||
hal.storage->write_byte(mem, temp.p1);
|
||||
|
||||
mem++;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.alt);
|
||||
hal.storage->write_dword(mem, temp.alt);
|
||||
|
||||
mem += 4;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.lat);
|
||||
hal.storage->write_dword(mem, temp.lat);
|
||||
|
||||
mem += 4;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.lng);
|
||||
hal.storage->write_dword(mem, temp.lng);
|
||||
}
|
||||
|
||||
/*
|
||||
|
||||
@ -363,7 +363,8 @@ static void do_set_home()
|
||||
|
||||
static void do_set_servo()
|
||||
{
|
||||
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
|
||||
hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
|
||||
hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
|
||||
}
|
||||
|
||||
static void do_set_relay()
|
||||
|
||||
@ -50,7 +50,7 @@ static void process_next_command()
|
||||
// and loads conditional or immediate commands if applicable
|
||||
|
||||
struct Location temp;
|
||||
byte old_index = 0;
|
||||
uint8_t old_index = 0;
|
||||
|
||||
// these are Navigation/Must commands
|
||||
// ---------------------------------
|
||||
|
||||
15
APMrover2/compat.h
Normal file
15
APMrover2/compat.h
Normal file
@ -0,0 +1,15 @@
|
||||
|
||||
#ifndef __COMPAT_H__
|
||||
#define __COMPAT_H__
|
||||
|
||||
#define OUTPUT GPIO_OUTPUT
|
||||
#define INPUT GPIO_INPUT
|
||||
|
||||
#define HIGH 1
|
||||
#define LOW 0
|
||||
|
||||
/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
|
||||
static void run_cli(AP_HAL::UARTDriver *port);
|
||||
|
||||
#endif // __COMPAT_H__
|
||||
|
||||
37
APMrover2/compat.pde
Normal file
37
APMrover2/compat.pde
Normal file
@ -0,0 +1,37 @@
|
||||
|
||||
|
||||
void delay(uint32_t ms)
|
||||
{
|
||||
hal.scheduler->delay(ms);
|
||||
}
|
||||
|
||||
void mavlink_delay(uint32_t ms)
|
||||
{
|
||||
hal.scheduler->delay(ms);
|
||||
}
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
return hal.scheduler->millis();
|
||||
}
|
||||
|
||||
uint32_t micros()
|
||||
{
|
||||
return hal.scheduler->micros();
|
||||
}
|
||||
|
||||
void pinMode(uint8_t pin, uint8_t output)
|
||||
{
|
||||
hal.gpio->pinMode(pin, output);
|
||||
}
|
||||
|
||||
void digitalWrite(uint8_t pin, uint8_t out)
|
||||
{
|
||||
hal.gpio->write(pin,out);
|
||||
}
|
||||
|
||||
uint8_t digitalRead(uint8_t pin)
|
||||
{
|
||||
return hal.gpio->read(pin);
|
||||
}
|
||||
|
||||
@ -42,19 +42,11 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// APM HARDWARE
|
||||
//
|
||||
|
||||
#ifndef CONFIG_APM_HARDWARE
|
||||
# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// APM2 HARDWARE DEFAULTS
|
||||
//
|
||||
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
|
||||
# define CONFIG_PUSHBUTTON DISABLED
|
||||
# define CONFIG_RELAY DISABLED
|
||||
@ -66,7 +58,7 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// LED and IO Pins
|
||||
//
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
# define A_LED_PIN 37
|
||||
# define B_LED_PIN 36
|
||||
# define C_LED_PIN 35
|
||||
@ -78,7 +70,7 @@
|
||||
# define CONFIG_RELAY ENABLED
|
||||
# define BATTERY_PIN_1 0
|
||||
# define CURRENT_PIN_1 1
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||
# define A_LED_PIN 27
|
||||
# define B_LED_PIN 26
|
||||
# define C_LED_PIN 25
|
||||
@ -90,9 +82,21 @@
|
||||
# define USB_MUX_PIN 23
|
||||
# define BATTERY_PIN_1 1
|
||||
# define CURRENT_PIN_1 2
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
# define A_LED_PIN 27
|
||||
# define B_LED_PIN 26
|
||||
# define C_LED_PIN 25
|
||||
# define LED_ON LOW
|
||||
# define LED_OFF HIGH
|
||||
# define SLIDE_SWITCH_PIN (-1)
|
||||
# define PUSHBUTTON_PIN (-1)
|
||||
# define CLI_SLIDER_ENABLED DISABLED
|
||||
# define USB_MUX_PIN -1
|
||||
# define BATTERY_PIN_1 1
|
||||
# define CURRENT_PIN_1 2
|
||||
#endif
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
#define CONFIG_SONAR DISABLED
|
||||
#endif
|
||||
|
||||
@ -139,7 +143,7 @@
|
||||
# endif
|
||||
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
|
||||
# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN
|
||||
# define CONFIG_SONAR_SOURCE_ANALOG_PIN A0
|
||||
# define CONFIG_SONAR_SOURCE_ANALOG_PIN 0
|
||||
# endif
|
||||
#else
|
||||
# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
static void read_control_switch()
|
||||
{
|
||||
|
||||
byte switchPosition = readSwitch();
|
||||
uint8_t switchPosition = readSwitch();
|
||||
|
||||
// If switchPosition = 255 this indicates that the mode control channel input was out of range
|
||||
// If we get this value we do not want to change modes.
|
||||
@ -17,7 +17,7 @@ static void read_control_switch()
|
||||
// as a spring loaded trainer switch).
|
||||
if (oldSwitchPosition != switchPosition ||
|
||||
(g.reset_switch_chan != 0 &&
|
||||
APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
|
||||
hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
|
||||
|
||||
set_mode(flight_modes[switchPosition]);
|
||||
|
||||
@ -32,8 +32,8 @@ static void read_control_switch()
|
||||
|
||||
}
|
||||
|
||||
static byte readSwitch(void){
|
||||
uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
|
||||
static uint8_t readSwitch(void){
|
||||
uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
|
||||
if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition
|
||||
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
|
||||
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
|
||||
|
||||
@ -215,9 +215,6 @@ enum gcs_severity {
|
||||
#define CONFIG_INS_OILPAN 1
|
||||
#define CONFIG_INS_MPU6000 2
|
||||
|
||||
#define APM_HARDWARE_APM1 1
|
||||
#define APM_HARDWARE_APM2 2
|
||||
|
||||
#define AP_BARO_BMP085 1
|
||||
#define AP_BARO_MS5611 2
|
||||
|
||||
|
||||
@ -32,7 +32,7 @@ static void failsafe_long_on_event(int fstype)
|
||||
{
|
||||
// This is how to handle a long loss of control signal failsafe.
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
|
||||
APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
|
||||
hal.rcin->clear_overrides();
|
||||
failsafe = fstype;
|
||||
switch(control_mode)
|
||||
{
|
||||
@ -94,9 +94,9 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_
|
||||
|
||||
if (event_id >= CH_5 && event_id <= CH_8) {
|
||||
if(event_repeat%2) {
|
||||
APM_RC.OutputCh(event_id, event_value); // send to Servos
|
||||
hal.rcout->write(event_id, event_value); // send to Servos
|
||||
} else {
|
||||
APM_RC.OutputCh(event_id, event_undo_value);
|
||||
hal.rcout->write(event_id, event_undo_value);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -38,9 +38,14 @@ void failsafe_check(uint32_t tnow)
|
||||
if (in_failsafe && tnow - last_timestamp > 20000) {
|
||||
// pass RC inputs to outputs every 20ms
|
||||
last_timestamp = tnow;
|
||||
APM_RC.clearOverride();
|
||||
for (uint8_t ch=0; ch<8; ch++) {
|
||||
APM_RC.OutputCh(ch, APM_RC.InputCh(ch));
|
||||
hal.rcin->clear_overrides();
|
||||
uint8_t start_ch = 0;
|
||||
if (demoing_servos) {
|
||||
start_ch = 1;
|
||||
}
|
||||
for (uint8_t ch=start_ch; ch<4; ch++) {
|
||||
hal.rcout->write(ch, hal.rcin->read(ch));
|
||||
}
|
||||
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,53 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
// These are function definitions so the Menu can be constructed before the functions
|
||||
// are defined below. Order matters to the compiler.
|
||||
static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
// Creates a constant array of structs representing menu options
|
||||
// and stores them in Flash memory, not RAM.
|
||||
// User enters the string in the console to call the functions on the right.
|
||||
// See class Menu in AP_Common for implementation details
|
||||
static const struct Menu::command planner_menu_commands[] PROGMEM = {
|
||||
{"gcs", planner_gcs},
|
||||
};
|
||||
|
||||
// A Macro to create the Menu
|
||||
MENU(planner_menu, "planner", planner_menu_commands);
|
||||
|
||||
static int8_t
|
||||
planner_mode(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
|
||||
planner_menu.run();
|
||||
return 0;
|
||||
}
|
||||
static int8_t
|
||||
planner_gcs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
gcs0.init(&Serial);
|
||||
|
||||
#if USB_MUX_PIN < 0
|
||||
// we don't have gcs3 if we have the USB mux setup
|
||||
gcs3.init(&Serial3);
|
||||
#endif
|
||||
|
||||
int loopcount = 0;
|
||||
while (1) {
|
||||
if (millis()-fast_loopTimer > 19) {
|
||||
fast_loopTimer = millis();
|
||||
|
||||
gcs_update();
|
||||
|
||||
read_radio();
|
||||
|
||||
gcs_data_stream_send();
|
||||
if ((loopcount % 16) == 0) { // 3 hz
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
}
|
||||
loopcount++;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
//Function that will read the radio data, limit servos and trigger a failsafe
|
||||
// ----------------------------------------------------------------------------
|
||||
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
|
||||
static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
|
||||
|
||||
|
||||
static void init_rc_in()
|
||||
@ -30,52 +30,50 @@ static void init_rc_in()
|
||||
|
||||
static void init_rc_out()
|
||||
{
|
||||
APM_RC.Init( &isr_registry ); // APM Radio initialization
|
||||
|
||||
APM_RC.enable_out(CH_1);
|
||||
APM_RC.enable_out(CH_2);
|
||||
APM_RC.enable_out(CH_3);
|
||||
APM_RC.enable_out(CH_4);
|
||||
APM_RC.enable_out(CH_5);
|
||||
APM_RC.enable_out(CH_6);
|
||||
APM_RC.enable_out(CH_7);
|
||||
APM_RC.enable_out(CH_8);
|
||||
hal.rcout->enable_ch(CH_1);
|
||||
hal.rcout->enable_ch(CH_2);
|
||||
hal.rcout->enable_ch(CH_3);
|
||||
hal.rcout->enable_ch(CH_4);
|
||||
hal.rcout->enable_ch(CH_5);
|
||||
hal.rcout->enable_ch(CH_6);
|
||||
hal.rcout->enable_ch(CH_7);
|
||||
hal.rcout->enable_ch(CH_8);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
|
||||
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
|
||||
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_trim);
|
||||
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
|
||||
hal.rcout->write(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
|
||||
hal.rcout->write(CH_2, g.channel_pitch.radio_trim);
|
||||
hal.rcout->write(CH_3, g.channel_throttle.radio_trim);
|
||||
hal.rcout->write(CH_4, g.channel_rudder.radio_trim);
|
||||
|
||||
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
|
||||
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
|
||||
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
|
||||
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
|
||||
hal.rcout->write(CH_5, g.rc_5.radio_trim);
|
||||
hal.rcout->write(CH_6, g.rc_6.radio_trim);
|
||||
hal.rcout->write(CH_7, g.rc_7.radio_trim);
|
||||
hal.rcout->write(CH_8, g.rc_8.radio_trim);
|
||||
#else
|
||||
APM_RC.OutputCh(CH_1, 1500); // Initialization of servo outputs
|
||||
APM_RC.OutputCh(CH_2, 1500);
|
||||
APM_RC.OutputCh(CH_3, 1000);
|
||||
APM_RC.OutputCh(CH_4, 1500);
|
||||
hal.rcout->write(CH_1, 1500); // Initialization of servo outputs
|
||||
hal.rcout->write(CH_2, 1500);
|
||||
hal.rcout->write(CH_3, 1000);
|
||||
hal.rcout->write(CH_4, 1500);
|
||||
|
||||
APM_RC.OutputCh(CH_5, 1500);
|
||||
APM_RC.OutputCh(CH_6, 1500);
|
||||
APM_RC.OutputCh(CH_7, 1500);
|
||||
APM_RC.OutputCh(CH_8, 2000);
|
||||
hal.rcout->write(CH_5, 1500);
|
||||
hal.rcout->write(CH_6, 1500);
|
||||
hal.rcout->write(CH_7, 1500);
|
||||
hal.rcout->write(CH_8, 2000);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
static void read_radio()
|
||||
{
|
||||
g.channel_roll.set_pwm(APM_RC.InputCh(CH_ROLL));
|
||||
g.channel_pitch.set_pwm(APM_RC.InputCh(CH_PITCH));
|
||||
g.channel_roll.set_pwm(hal.rcin->read(CH_ROLL));
|
||||
g.channel_pitch.set_pwm(hal.rcin->read(CH_PITCH));
|
||||
|
||||
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
|
||||
g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
|
||||
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
|
||||
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
|
||||
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
|
||||
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
|
||||
g.channel_throttle.set_pwm(hal.rcout->read(CH_3));
|
||||
g.channel_rudder.set_pwm(hal.rcout->read(CH_4));
|
||||
g.rc_5.set_pwm(hal.rcout->read(CH_5));
|
||||
g.rc_6.set_pwm(hal.rcout->read(CH_6));
|
||||
g.rc_7.set_pwm(hal.rcout->read(CH_7));
|
||||
g.rc_8.set_pwm(hal.rcout->read(CH_8));
|
||||
|
||||
control_failsafe(g.channel_throttle.radio_in);
|
||||
|
||||
|
||||
@ -28,15 +28,17 @@ static void read_battery(void)
|
||||
return;
|
||||
}
|
||||
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1);
|
||||
battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average());
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
// this copes with changing the pin at runtime
|
||||
batt_volt_pin->set_pin(g.battery_volt_pin);
|
||||
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin->read_average());
|
||||
}
|
||||
if(g.battery_monitoring == 4) {
|
||||
// this copes with changing the pin at runtime
|
||||
batt_curr_pin->set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_pin->read_average());
|
||||
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
|
||||
}
|
||||
if(g.battery_monitoring == 4) {
|
||||
static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1);
|
||||
current_amps1 = CURRENT_AMPS(current_pin.read_average());
|
||||
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
|
||||
}
|
||||
|
||||
#if BATTERY_EVENT == ENABLED
|
||||
if(battery_voltage1 < LOW_VOLTAGE) low_battery_event();
|
||||
|
||||
@ -169,7 +169,9 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
g.rc_8.update_min_max();
|
||||
|
||||
if(cliSerial->available() > 0){
|
||||
cliSerial->flush();
|
||||
while (cliSerial->available() > 0) {
|
||||
cliSerial->read();
|
||||
}
|
||||
g.channel_roll.save_eeprom();
|
||||
g.channel_pitch.save_eeprom();
|
||||
g.channel_throttle.save_eeprom();
|
||||
@ -191,7 +193,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
byte switchPosition, mode = 0;
|
||||
uint8_t switchPosition, mode = 0;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
|
||||
print_hit_enter();
|
||||
@ -294,34 +296,14 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
|
||||
handle full accelerometer calibration via user dialog
|
||||
*/
|
||||
#if !defined( __AVR_ATmega1280__ )
|
||||
static void setup_printf_P(const prog_char_t *fmt, ...)
|
||||
{
|
||||
va_list arg_list;
|
||||
va_start(arg_list, fmt);
|
||||
cliSerial->vprintf_P(fmt, arg_list);
|
||||
va_end(arg_list);
|
||||
}
|
||||
|
||||
static void setup_wait_key(void)
|
||||
{
|
||||
// wait for user input
|
||||
while (!cliSerial->available()) {
|
||||
delay(20);
|
||||
}
|
||||
// clear input buffer
|
||||
while( cliSerial->available() ) {
|
||||
cliSerial->read();
|
||||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
cliSerial->println_P(PSTR("Initialising gyros"));
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
delay, flash_leds, &timer_scheduler);
|
||||
if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
|
||||
flash_leds);
|
||||
if (ins.calibrate_accel(flash_leds, hal.console)) {
|
||||
if (g.manual_level == 0) {
|
||||
cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));
|
||||
g.manual_level.set_and_save(1);
|
||||
@ -538,7 +520,7 @@ print_radio_values()
|
||||
}
|
||||
|
||||
static void
|
||||
print_switch(byte p, byte m)
|
||||
print_switch(uint8_t p, uint8_t m)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Pos %d: "),p);
|
||||
print_flight_mode(m);
|
||||
@ -597,10 +579,10 @@ radio_input_switch(void)
|
||||
|
||||
static void zero_eeprom(void)
|
||||
{
|
||||
byte b = 0;
|
||||
uint8_t b = 0;
|
||||
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
|
||||
for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
|
||||
eeprom_write_byte((uint8_t *) i, b);
|
||||
for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) {
|
||||
hal.storage->write_byte(i, b);
|
||||
}
|
||||
cliSerial->printf_P(PSTR("done\n"));
|
||||
}
|
||||
@ -614,18 +596,4 @@ static void print_enabled(bool b)
|
||||
cliSerial->printf_P(PSTR("abled\n"));
|
||||
}
|
||||
|
||||
static void
|
||||
print_accel_offsets_and_scaling(void)
|
||||
{
|
||||
Vector3f accel_offsets = ins.get_accel_offsets();
|
||||
Vector3f accel_scale = ins.get_accel_scale();
|
||||
cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
|
||||
(float)accel_offsets.x, // Pitch
|
||||
(float)accel_offsets.y, // Roll
|
||||
(float)accel_offsets.z, // YAW
|
||||
(float)accel_scale.x, // Pitch
|
||||
(float)accel_scale.y, // Roll
|
||||
(float)accel_scale.z); // YAW
|
||||
}
|
||||
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
@ -14,7 +14,6 @@ static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
|
||||
#endif
|
||||
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
|
||||
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
|
||||
static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
|
||||
|
||||
// This is the help function
|
||||
// PSTR is an AVR macro to read strings from flash memory
|
||||
@ -40,18 +39,17 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||
#endif
|
||||
{"setup", setup_mode},
|
||||
{"test", test_mode},
|
||||
{"help", main_menu_help},
|
||||
{"planner", planner_mode}
|
||||
{"help", main_menu_help}
|
||||
};
|
||||
|
||||
// Create the top-level menu object.
|
||||
MENU(main_menu, THISFIRMWARE, main_menu_commands);
|
||||
|
||||
// the user wants the CLI. It never exits
|
||||
static void run_cli(FastSerial *port)
|
||||
static void run_cli(AP_HAL::UARTDriver *port)
|
||||
{
|
||||
// disable the failsafe code in the CLI
|
||||
timer_scheduler.set_failsafe(NULL);
|
||||
hal.scheduler->register_timer_failsafe(NULL,1);
|
||||
|
||||
cliSerial = port;
|
||||
Menu::set_port(port);
|
||||
@ -93,7 +91,7 @@ static void init_ardupilot()
|
||||
// XXX This could be optimised to reduce the buffer sizes in the cases
|
||||
// where they are not otherwise required.
|
||||
//
|
||||
cliSerial->begin(SERIAL0_BAUD, 128, 128);
|
||||
hal.uartA->begin(SERIAL0_BAUD, 128, 128);
|
||||
|
||||
// GPS serial port.
|
||||
//
|
||||
@ -105,37 +103,12 @@ static void init_ardupilot()
|
||||
// on the message set configured.
|
||||
//
|
||||
// standard gps running
|
||||
Serial1.begin(115200, 128, 16);
|
||||
hal.uartB->begin(115200, 128, 16);
|
||||
|
||||
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
|
||||
"\n\nFree RAM: %u\n"),
|
||||
memcheck_available_memory());
|
||||
|
||||
//
|
||||
// Initialize Wire and SPI libraries
|
||||
//
|
||||
#ifndef DESKTOP_BUILD
|
||||
I2c.begin();
|
||||
I2c.timeOut(5);
|
||||
// initially set a fast I2c speed, and drop it on first failures
|
||||
I2c.setSpeed(true);
|
||||
#endif
|
||||
SPI.begin();
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
|
||||
//
|
||||
// Initialize the ISR registry.
|
||||
//
|
||||
isr_registry.init();
|
||||
|
||||
//
|
||||
// Initialize the timer scheduler to use the ISR registry.
|
||||
//
|
||||
|
||||
timer_scheduler.init( & isr_registry );
|
||||
|
||||
// initialise the analog port reader
|
||||
AP_AnalogSource_Arduino::init_timer(&timer_scheduler);
|
||||
|
||||
//
|
||||
// Check the EEPROM format version before loading any parameters from EEPROM.
|
||||
//
|
||||
@ -147,18 +120,22 @@ static void init_ardupilot()
|
||||
g.num_resets.set_and_save(g.num_resets+1);
|
||||
|
||||
// init the GCS
|
||||
gcs0.init(&Serial);
|
||||
gcs0.init(hal.uartA);
|
||||
|
||||
// Register mavlink_delay_cb, which will run anytime you have
|
||||
// more than 5ms remaining in your call to hal.scheduler->delay
|
||||
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
|
||||
|
||||
#if USB_MUX_PIN > 0
|
||||
if (!usb_connected) {
|
||||
// we are not connected via USB, re-init UART0 with right
|
||||
// baud rate
|
||||
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
|
||||
}
|
||||
#else
|
||||
// we have a 2nd serial port for telemetry
|
||||
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
gcs3.init(&Serial3);
|
||||
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
gcs3.init(hal.uartC);
|
||||
#endif
|
||||
|
||||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
@ -182,7 +159,7 @@ static void init_ardupilot()
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
#if CONFIG_ADC == ENABLED
|
||||
adc.Init(&timer_scheduler); // APM ADC library initialization
|
||||
adc.Init(); // APM ADC library initialization
|
||||
#endif
|
||||
|
||||
#if LITE == DISABLED
|
||||
@ -239,15 +216,14 @@ static void init_ardupilot()
|
||||
// Do GPS init
|
||||
g_gps = &g_gps_driver;
|
||||
// GPS initialisation
|
||||
g_gps->init(GPS::GPS_ENGINE_AUTOMOTIVE);
|
||||
g_gps->init(hal.uartB, GPS::GPS_ENGINE_AUTOMOTIVE);
|
||||
|
||||
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
|
||||
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
|
||||
mavlink_system.type = MAV_TYPE_GROUND_ROVER;
|
||||
|
||||
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
|
||||
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
|
||||
|
||||
RC_Channel::set_apm_rc( &APM_RC ); // Provide reference to RC outputs.
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
init_rc_out(); // sets up the timer libs
|
||||
|
||||
@ -261,15 +237,14 @@ static void init_ardupilot()
|
||||
pinMode(PUSHBUTTON_PIN, INPUT); // unused
|
||||
#endif
|
||||
#if CONFIG_RELAY == ENABLED
|
||||
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
|
||||
relay.init();
|
||||
#endif
|
||||
|
||||
/*
|
||||
setup the 'main loop is dead' check. Note that this relies on
|
||||
the RC library being initialised.
|
||||
*/
|
||||
timer_scheduler.set_failsafe(failsafe_check);
|
||||
|
||||
hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
|
||||
|
||||
// If the switch is in 'menu' mode, run the main menu.
|
||||
//
|
||||
@ -293,7 +268,7 @@ static void init_ardupilot()
|
||||
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
|
||||
cliSerial->println_P(msg);
|
||||
#if USB_MUX_PIN == 0
|
||||
Serial3.println_P(msg);
|
||||
hal.uartC->println_P(msg);
|
||||
#endif
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
@ -353,7 +328,7 @@ static void startup_ground(void)
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
|
||||
}
|
||||
|
||||
static void set_mode(byte mode)
|
||||
static void set_mode(uint8_t mode)
|
||||
{
|
||||
|
||||
if(control_mode == mode){
|
||||
@ -449,12 +424,12 @@ static void startup_INS_ground(bool force_accel_level)
|
||||
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
mavlink_delay, flash_leds, &timer_scheduler);
|
||||
flash_leds);
|
||||
if (force_accel_level || g.manual_level == 0) {
|
||||
// when MANUAL_LEVEL is set to 1 we don't do accelerometer
|
||||
// levelling on each boot, and instead rely on the user to do
|
||||
// it once via the ground station
|
||||
ins.init_accel(mavlink_delay, flash_leds);
|
||||
ins.init_accel(flash_leds);
|
||||
}
|
||||
ahrs.set_fly_forward(true);
|
||||
ahrs.reset();
|
||||
@ -538,9 +513,9 @@ static void check_usb_mux(void)
|
||||
// the user has switched to/from the telemetry port
|
||||
usb_connected = usb_check;
|
||||
if (usb_connected) {
|
||||
cliSerial->begin(SERIAL0_BAUD, 128, 128);
|
||||
hal.uartA->begin(SERIAL0_BAUD, 128, 128);
|
||||
} else {
|
||||
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@ -556,17 +531,13 @@ void flash_leds(bool on)
|
||||
digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
|
||||
}
|
||||
|
||||
#ifndef DESKTOP_BUILD
|
||||
/*
|
||||
* Read Vcc vs 1.1v internal reference
|
||||
*/
|
||||
uint16_t board_voltage(void)
|
||||
{
|
||||
static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC);
|
||||
return vcc.read_vcc();
|
||||
return vcc_pin->read_latest();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
static void
|
||||
print_flight_mode(uint8_t mode)
|
||||
|
||||
@ -20,9 +20,6 @@ static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
@ -38,8 +35,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"battery", test_battery},
|
||||
{"relay", test_relay},
|
||||
{"waypoints", test_wp},
|
||||
{"xbee", test_xbee},
|
||||
{"eedump", test_eedump},
|
||||
{"modeswitch", test_modeswitch},
|
||||
|
||||
// Tests below here are for hardware sensors only present
|
||||
@ -49,7 +44,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"adc", test_adc},
|
||||
#endif
|
||||
{"gps", test_gps},
|
||||
{"rawgps", test_rawgps},
|
||||
{"ins", test_ins},
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
{"sonartest", test_sonar},
|
||||
@ -82,21 +76,6 @@ static void print_hit_enter()
|
||||
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_eedump(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
intptr_t i, j;
|
||||
|
||||
// hexdump the EEPROM
|
||||
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
|
||||
cliSerial->printf_P(PSTR("%04x:"), i);
|
||||
for (j = 0; j < 16; j++)
|
||||
cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
|
||||
cliSerial->println();
|
||||
}
|
||||
return(0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
@ -137,12 +116,12 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
|
||||
delay(20);
|
||||
|
||||
// New radio frame? (we could use also if((millis()- timer) > 20)
|
||||
if (APM_RC.GetState() == 1){
|
||||
if (hal.rcin->valid() > 0) {
|
||||
cliSerial->print("CH:");
|
||||
for(int i = 0; i < 8; i++){
|
||||
cliSerial->print(APM_RC.InputCh(i)); // Print channel values
|
||||
cliSerial->print(hal.rcout->read(i)); // Print channel values
|
||||
cliSerial->print(",");
|
||||
APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
|
||||
hal.rcout->write(i, hal.rcout->read(i)); // Copy input to Servos
|
||||
}
|
||||
cliSerial->println();
|
||||
}
|
||||
@ -198,7 +177,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
test_failsafe(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
byte fail_test;
|
||||
uint8_t fail_test;
|
||||
print_hit_enter();
|
||||
for(int i = 0; i < 50; i++){
|
||||
delay(20);
|
||||
@ -317,7 +296,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
|
||||
cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
|
||||
|
||||
for(byte i = 0; i <= g.command_total; i++){
|
||||
for(uint8_t i = 0; i <= g.command_total; i++){
|
||||
struct Location temp = get_cmd_with_index(i);
|
||||
test_wp_print(&temp, i);
|
||||
}
|
||||
@ -326,7 +305,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
static void
|
||||
test_wp_print(struct Location *cmd, byte wp_index)
|
||||
test_wp_print(struct Location *cmd, uint8_t wp_index)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
|
||||
(int)wp_index,
|
||||
@ -338,25 +317,6 @@ test_wp_print(struct Location *cmd, byte wp_index)
|
||||
cmd->lng);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_xbee(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
|
||||
|
||||
while(1){
|
||||
|
||||
if (Serial3.available())
|
||||
Serial3.write(Serial3.read());
|
||||
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
@ -369,7 +329,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
byte switchPosition = readSwitch();
|
||||
uint8_t switchPosition = readSwitch();
|
||||
if (oldSwitchPosition != switchPosition){
|
||||
cliSerial->printf_P(PSTR("Position %d\n"), switchPosition);
|
||||
oldSwitchPosition = switchPosition;
|
||||
@ -415,7 +375,7 @@ static int8_t
|
||||
test_adc(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
adc.Init(&timer_scheduler);
|
||||
adc.Init();
|
||||
delay(1000);
|
||||
cliSerial->printf_P(PSTR("ADC\n"));
|
||||
delay(1000);
|
||||
@ -467,7 +427,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
||||
//cliSerial->printf_P(PSTR("Calibrating."));
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
delay, flash_leds, &timer_scheduler);
|
||||
flash_leds);
|
||||
ahrs.reset();
|
||||
|
||||
print_hit_enter();
|
||||
@ -530,7 +490,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
// we need the AHRS initialised for this test
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
delay, flash_leds, &timer_scheduler);
|
||||
flash_leds);
|
||||
ahrs.reset();
|
||||
|
||||
int counter = 0;
|
||||
@ -597,31 +557,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// real sensors that have not been simulated yet go here
|
||||
|
||||
#if HIL_MODE == HIL_MODE_DISABLED
|
||||
|
||||
static int8_t
|
||||
test_rawgps(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
if (Serial3.available()){
|
||||
digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS
|
||||
Serial1.write(Serial3.read());
|
||||
digitalWrite(B_LED_PIN, LED_OFF);
|
||||
}
|
||||
if (Serial1.available()){
|
||||
digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS
|
||||
Serial3.write(Serial1.read());
|
||||
digitalWrite(C_LED_PIN, LED_OFF);
|
||||
}
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
static int8_t
|
||||
test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
@ -634,7 +569,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
while(1){
|
||||
delay(20);
|
||||
if(g.sonar_enabled){
|
||||
sonar_dist = sonar.read();
|
||||
sonar_dist = sonar->read();
|
||||
}
|
||||
cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
|
||||
|
||||
@ -645,6 +580,5 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
return (0);
|
||||
}
|
||||
#endif // SONAR == ENABLED
|
||||
#endif // HIL_MODE == HIL_MODE_DISABLED
|
||||
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
Loading…
Reference in New Issue
Block a user