int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
channel_steer->get_control_in(),
g.rc_2.get_control_in(),
channel_throttle->get_control_in(),
g.rc_4.get_control_in(),
g.rc_5.get_control_in(),
g.rc_6.get_control_in(),
g.rc_7.get_control_in(),
g.rc_8.get_control_in());
if(cliSerial->available() > 0){
return (0);
}
}
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void Rover::startup_ground(void)
{
set_mode(INITIALISING);
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
#if(GROUND_START_DELAY > 0)
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With delay");
delay(GROUND_START_DELAY * 1000);
#endif
//IMU ground start
//------------------------
//
startup_INS_ground();
// read the radio to set trims
// ---------------------------
trim_radio();
// initialise mission library
mission.init();
// we don't want writes to the serial port to cause us to pause
// so set serial ports non-blocking once we are ready to drive
serial_manager.set_blocking_writes_all(false);
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
gcs_send_text(MAV_SEVERITY_INFO,"Ready to drive");
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void Plane::startup_ground(void)
{
set_mode(INITIALISING);
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if (GROUND_START_DELAY > 0)
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay"));
delay(GROUND_START_DELAY * 1000);
#endif
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
if (!g.skip_gyro_cal) {
demo_servos(1);
}
//INS ground start
//------------------------
//
startup_INS_ground();
// read the radio to set trims
// ---------------------------
if (g.trim_rc_at_start != 0) {
trim_radio();
}
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialise mission library
mission.init();
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
if (!g.skip_gyro_cal) {
demo_servos(3);
}
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
failsafe.last_heartbeat_ms = millis();
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test = 0;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->println("Unplug battery, throttle in neutral, turn off radio.");
while(channel_throttle->get_control_in() > 0){
delay(20);
read_radio();
}
while(1){
delay(20);
read_radio();
if(channel_throttle->get_control_in() > 0){
cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->get_control_in());
fail_test++;
}
if (oldSwitchPosition != readSwitch()){
cliSerial->print("CONTROL MODE CHANGED: ");
print_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(throttle_failsafe_active()) {
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in());
print_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(fail_test > 0){
return (0);
}
if(cliSerial->available() > 0){
cliSerial->println("LOS caused no change in APM.");
return (0);
}
}
}
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test;
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(channel_throttle->control_in > 0) {
hal.scheduler->delay(20);
read_radio();
}
while(1) {
hal.scheduler->delay(20);
read_radio();
if(channel_throttle->control_in > 0) {
cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)channel_throttle->control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()) {
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(rc_failsafe_active()) {
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)channel_throttle->radio_in);
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(fail_test > 0) {
return (0);
}
if(cliSerial->available() > 0) {
cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
return (0);
}
}
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void Plane::startup_ground(void)
{
set_mode(INITIALISING, MODE_REASON_UNKNOWN);
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
#if (GROUND_START_DELAY > 0)
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With delay");
delay(GROUND_START_DELAY * 1000);
#endif
//INS ground start
//------------------------
//
startup_INS_ground();
// read the radio to set trims
// ---------------------------
if (g.trim_rc_at_start != 0) {
trim_radio();
}
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialise mission library
mission.init();
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
failsafe.last_heartbeat_ms = millis();
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
gcs_send_text(MAV_SEVERITY_INFO,"Ready to fly");
}
int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1) {
hal.scheduler->delay(20);
read_radio();
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
channel_throttle->calc_pwm();
channel_rudder->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
(int)channel_roll->control_in,
(int)channel_pitch->control_in,
(int)channel_throttle->control_in,
(int)channel_rudder->control_in,
(int)g.rc_5.control_in,
(int)g.rc_6.control_in,
(int)g.rc_7.control_in,
(int)g.rc_8.control_in);
if(cliSerial->available() > 0) {
return (0);
}
}
}
