zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add trial period

master
yaozb 5 years ago
parent
9df69543f0
commit
ecdc425ce8
10 changed files with 6476 additions and 6402 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1716
      ArduCopter/AP_Arming.cpp
  2. 124
      ArduCopter/AP_Arming.h
  3. 4
      ArduCopter/Copter.h
  4. 3281
      ArduCopter/Parameters.cpp
  5. 1268
      ArduCopter/Parameters.h
  6. 1385
      ArduCopter/system.cpp
  7. 3339
      libraries/AP_GPS/AP_GPS.cpp
  8. 1176
      libraries/AP_GPS/AP_GPS.h
  9. 583
      libraries/AP_GPS/GPS_Backend.cpp
  10. 2
      modules/mavlink

1716
ArduCopter/AP_Arming.cpp
View File

File diff suppressed because it is too large Load Diff

124
ArduCopter/AP_Arming.h
Unescape View File

@ -1,62 +1,62 @@
#pragma once #pragma once
#include <AP_Arming/AP_Arming.h> #include <AP_Arming/AP_Arming.h>
class AP_Arming_Copter : public AP_Arming class AP_Arming_Copter : public AP_Arming
{ {
public: public:
friend class Copter; friend class Copter;
friend class ToyMode; friend class ToyMode;
AP_Arming_Copter() : AP_Arming() AP_Arming_Copter() : AP_Arming()
{ {
// default REQUIRE parameter to 1 (Copter does not have an // default REQUIRE parameter to 1 (Copter does not have an
// actual ARMING_REQUIRE parameter) // actual ARMING_REQUIRE parameter)
require.set_default((uint8_t)Required::YES_MIN_PWM); require.set_default((uint8_t)Required::YES_MIN_PWM);
} }
/* Do not allow copies */ /* Do not allow copies */
AP_Arming_Copter(const AP_Arming_Copter &other) = delete; AP_Arming_Copter(const AP_Arming_Copter &other) = delete;
AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete; AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
void update(void); void update(void);
bool rc_calibration_checks(bool display_failure) override; bool rc_calibration_checks(bool display_failure) override;
bool disarm() override; bool disarm() override;
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
protected: protected:
bool pre_arm_checks(bool display_failure) override; bool pre_arm_checks(bool display_failure) override;
bool pre_arm_ekf_attitude_check(); bool pre_arm_ekf_attitude_check();
bool pre_arm_terrain_check(bool display_failure); bool pre_arm_terrain_check(bool display_failure);
bool proximity_checks(bool display_failure) const override; bool proximity_checks(bool display_failure) const override;
bool arm_checks(AP_Arming::Method method) override; bool arm_checks(AP_Arming::Method method) override;
// mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced
bool mandatory_checks(bool display_failure) override; bool mandatory_checks(bool display_failure) override;
// NOTE! the following check functions *DO* call into AP_Arming: // NOTE! the following check functions *DO* call into AP_Arming:
bool ins_checks(bool display_failure) override; bool ins_checks(bool display_failure) override;
bool compass_checks(bool display_failure) override; bool compass_checks(bool display_failure) override;
bool gps_checks(bool display_failure) override; bool gps_checks(bool display_failure) override;
bool barometer_checks(bool display_failure) override; bool barometer_checks(bool display_failure) override;
bool board_voltage_checks(bool display_failure) override; bool board_voltage_checks(bool display_failure) override;
// NOTE! the following check functions *DO NOT* call into AP_Arming! // NOTE! the following check functions *DO NOT* call into AP_Arming!
bool parameter_checks(bool display_failure); bool parameter_checks(bool display_failure);
bool motor_checks(bool display_failure); bool motor_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure); bool pilot_throttle_checks(bool display_failure);
bool oa_checks(bool display_failure); bool oa_checks(bool display_failure);
bool mandatory_gps_checks(bool display_failure); bool mandatory_gps_checks(bool display_failure);
bool mandatory_deadline_checks(bool display_failure);
void set_pre_arm_check(bool b); void set_pre_arm_check(bool b);
private: private:
// actually contains the pre-arm checks. This is wrapped so that // actually contains the pre-arm checks. This is wrapped so that
// we can store away success/failure of the checks. // we can store away success/failure of the checks.
bool run_pre_arm_checks(bool display_failure); bool run_pre_arm_checks(bool display_failure);
}; };

4
ArduCopter/Copter.h
Unescape View File

@ -837,7 +837,7 @@ private:
void convert_lgr_parameters(void); void convert_lgr_parameters(void);
void convert_tradheli_parameters(void); void convert_tradheli_parameters(void);
void convert_fs_options_params(void); void convert_fs_options_params(void);
void get_deadline_params( int32_t &deadline );
// precision_landing.cpp // precision_landing.cpp
void init_precland(); void init_precland();
void update_precland(); void update_precland();
@ -895,7 +895,7 @@ private:
const char* get_frame_string(); const char* get_frame_string();
void allocate_motors(void); void allocate_motors(void);
bool is_tradheli() const; bool is_tradheli() const;
bool deadline_ok();
// terrain.cpp // terrain.cpp
void terrain_update(); void terrain_update();
void terrain_logging(); void terrain_logging();

3281
ArduCopter/Parameters.cpp
View File

File diff suppressed because it is too large Load Diff

1268
ArduCopter/Parameters.h
View File

File diff suppressed because it is too large Load Diff

1385
ArduCopter/system.cpp
View File

File diff suppressed because it is too large Load Diff

3339
libraries/AP_GPS/AP_GPS.cpp
View File

File diff suppressed because it is too large Load Diff

1176
libraries/AP_GPS/AP_GPS.h
View File

File diff suppressed because it is too large Load Diff

583
libraries/AP_GPS/GPS_Backend.cpp
Unescape View File

@ -1,291 +1,292 @@
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_GPS.h" #include "AP_GPS.h"
#include "GPS_Backend.h" #include "GPS_Backend.h"
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#define GPS_BACKEND_DEBUGGING 0 #define GPS_BACKEND_DEBUGGING 0
#if GPS_BACKEND_DEBUGGING #if GPS_BACKEND_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else #else
# define Debug(fmt, args ...) # define Debug(fmt, args ...)
#endif #endif
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
port(_port), port(_port),
gps(_gps), gps(_gps),
state(_state) state(_state)
{ {
state.have_speed_accuracy = false; state.have_speed_accuracy = false;
state.have_horizontal_accuracy = false; state.have_horizontal_accuracy = false;
state.have_vertical_accuracy = false; state.have_vertical_accuracy = false;
} }
int32_t AP_GPS_Backend::swap_int32(int32_t v) const int32_t AP_GPS_Backend::swap_int32(int32_t v) const
{ {
const uint8_t *b = (const uint8_t *)&v; const uint8_t *b = (const uint8_t *)&v;
union { union {
int32_t v; int32_t v;
uint8_t b[4]; uint8_t b[4];
} u; } u;
u.b[0] = b[3]; u.b[0] = b[3];
u.b[1] = b[2]; u.b[1] = b[2];
u.b[2] = b[1]; u.b[2] = b[1];
u.b[3] = b[0]; u.b[3] = b[0];
return u.v; return u.v;
} }
int16_t AP_GPS_Backend::swap_int16(int16_t v) const int16_t AP_GPS_Backend::swap_int16(int16_t v) const
{ {
const uint8_t *b = (const uint8_t *)&v; const uint8_t *b = (const uint8_t *)&v;
union { union {
int16_t v; int16_t v;
uint8_t b[2]; uint8_t b[2];
} u; } u;
u.b[0] = b[1]; u.b[0] = b[1];
u.b[1] = b[0]; u.b[1] = b[0];
return u.v; return u.v;
} }
/** /**
fill in time_week_ms and time_week from BCD date and time components fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time assumes MTK19 millisecond form of bcd_time
*/ */
void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
{ {
uint8_t year, mon, day, hour, min, sec; uint8_t year, mon, day, hour, min, sec;
uint16_t msec; uint16_t msec;
year = bcd_date % 100; year = bcd_date % 100;
mon = (bcd_date / 100) % 100; mon = (bcd_date / 100) % 100;
day = bcd_date / 10000; day = bcd_date / 10000;
uint32_t v = bcd_milliseconds; uint32_t v = bcd_milliseconds;
msec = v % 1000; v /= 1000; msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100; sec = v % 100; v /= 100;
min = v % 100; v /= 100; min = v % 100; v /= 100;
hour = v % 100; hour = v % 100;
int8_t rmon = mon - 2; int8_t rmon = mon - 2;
if (0 >= rmon) { if (0 >= rmon) {
rmon += 12; rmon += 12;
year -= 1; year -= 1;
} }
state.zr_date_timestamp =(year+2000)*10000+mon*100+day; //FIXME only calcute onecs
// get time in seconds since unix epoch
uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day; // get time in seconds since unix epoch
ret += year*365 + 10501; uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day;
ret = ret*24 + hour; ret += year*365 + 10501;
ret = ret*60 + min; ret = ret*24 + hour;
ret = ret*60 + sec; ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL; // convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
state.time_week = ret / AP_SEC_PER_WEEK; // get GPS week and time
state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; state.time_week = ret / AP_SEC_PER_WEEK;
state.time_week_ms += msec; state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC;
} state.time_week_ms += msec;
}
/*
fill in 3D velocity for a GPS that doesn't give vertical velocity numbers /*
*/ fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
void AP_GPS_Backend::fill_3d_velocity(void) */
{ void AP_GPS_Backend::fill_3d_velocity(void)
float gps_heading = radians(state.ground_course); {
float gps_heading = radians(state.ground_course);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading); state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.z = 0; state.velocity.y = state.ground_speed * sinf(gps_heading);
state.have_vertical_velocity = false; state.velocity.z = 0;
} state.have_vertical_velocity = false;
}
void
AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) void
{ AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
// not all backends have valid ports {
if (port != nullptr) { // not all backends have valid ports
if (port->txspace() > len) { if (port != nullptr) {
port->write(data, len); if (port->txspace() > len) {
} else { port->write(data, len);
Debug("GPS %d: Not enough TXSPACE", state.instance + 1); } else {
} Debug("GPS %d: Not enough TXSPACE", state.instance + 1);
} }
} }
}
void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const
{ void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const
const uint8_t instance = state.instance; {
const struct AP_GPS::detect_state dstate = gps.detect_state[instance]; const uint8_t instance = state.instance;
const struct AP_GPS::detect_state dstate = gps.detect_state[instance];
if (dstate.auto_detected_baud) {
hal.util->snprintf(buffer, buflen, if (dstate.auto_detected_baud) {
"GPS %d: detected as %s at %d baud", hal.util->snprintf(buffer, buflen,
instance + 1, "GPS %d: detected as %s at %d baud",
name(), instance + 1,
gps._baudrates[dstate.current_baud]); name(),
} else { gps._baudrates[dstate.current_baud]);
hal.util->snprintf(buffer, buflen, } else {
"GPS %d: specified as %s", hal.util->snprintf(buffer, buflen,
instance + 1, "GPS %d: specified as %s",
name()); instance + 1,
} name());
} }
}
void AP_GPS_Backend::broadcast_gps_type() const
{ void AP_GPS_Backend::broadcast_gps_type() const
#ifndef HAL_NO_GCS {
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; #ifndef HAL_NO_GCS
_detection_message(buffer, sizeof(buffer)); char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer); _detection_message(buffer, sizeof(buffer));
#endif gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer);
} #endif
}
void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
{ void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
#ifndef HAL_NO_LOGGING {
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; #ifndef HAL_NO_LOGGING
_detection_message(buffer, sizeof(buffer)); char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
AP::logger().Write_Message(buffer); _detection_message(buffer, sizeof(buffer));
#endif AP::logger().Write_Message(buffer);
} #endif
}
bool AP_GPS_Backend::should_log() const
{ bool AP_GPS_Backend::should_log() const
return gps.should_log(); {
} return gps.should_log();
}
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
{ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
#ifndef HAL_NO_GCS {
const uint8_t instance = state.instance; #ifndef HAL_NO_GCS
// send status const uint8_t instance = state.instance;
switch (instance) { // send status
case 0: switch (instance) {
mavlink_msg_gps_rtk_send(chan, case 0:
0, // Not implemented yet mavlink_msg_gps_rtk_send(chan,
0, // Not implemented yet 0, // Not implemented yet
state.rtk_week_number, 0, // Not implemented yet
state.rtk_time_week_ms, state.rtk_week_number,
0, // Not implemented yet state.rtk_time_week_ms,
0, // Not implemented yet 0, // Not implemented yet
state.rtk_num_sats, 0, // Not implemented yet
state.rtk_baseline_coords_type, state.rtk_num_sats,
state.rtk_baseline_x_mm, state.rtk_baseline_coords_type,
state.rtk_baseline_y_mm, state.rtk_baseline_x_mm,
state.rtk_baseline_z_mm, state.rtk_baseline_y_mm,
state.rtk_accuracy, state.rtk_baseline_z_mm,
state.rtk_iar_num_hypotheses); state.rtk_accuracy,
break; state.rtk_iar_num_hypotheses);
case 1: break;
mavlink_msg_gps2_rtk_send(chan, case 1:
0, // Not implemented yet mavlink_msg_gps2_rtk_send(chan,
0, // Not implemented yet 0, // Not implemented yet
state.rtk_week_number, 0, // Not implemented yet
state.rtk_time_week_ms, state.rtk_week_number,
0, // Not implemented yet state.rtk_time_week_ms,
0, // Not implemented yet 0, // Not implemented yet
state.rtk_num_sats, 0, // Not implemented yet
state.rtk_baseline_coords_type, state.rtk_num_sats,
state.rtk_baseline_x_mm, state.rtk_baseline_coords_type,
state.rtk_baseline_y_mm, state.rtk_baseline_x_mm,
state.rtk_baseline_z_mm, state.rtk_baseline_y_mm,
state.rtk_accuracy, state.rtk_baseline_z_mm,
state.rtk_iar_num_hypotheses); state.rtk_accuracy,
break; state.rtk_iar_num_hypotheses);
} break;
#endif }
} #endif
}
/*
set a timestamp based on arrival time on uart at current byte, /*
assuming the message started nbytes ago set a timestamp based on arrival time on uart at current byte,
*/ assuming the message started nbytes ago
void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes) */
{ void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
if (port) { {
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U; if (port) {
} state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
} }
}
void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
{ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
if (itow != _last_itow) { {
_last_itow = itow; if (itow != _last_itow) {
_last_itow = itow;
/*
we need to calculate a pseudo-itow, which copes with the /*
iTow from the GPS changing in unexpected ways. We assume we need to calculate a pseudo-itow, which copes with the
that timestamps from the GPS are always in multiples of iTow from the GPS changing in unexpected ways. We assume
50ms. That means we can't handle a GPS with an update rate that timestamps from the GPS are always in multiples of
of more than 20Hz. We could do more, but we'd need the GPS 50ms. That means we can't handle a GPS with an update rate
poll time to be higher of more than 20Hz. We could do more, but we'd need the GPS
*/ poll time to be higher
const uint32_t gps_min_period_ms = 50; */
const uint32_t gps_min_period_ms = 50;
// get the time the packet arrived on the UART
uint64_t uart_us = port->receive_time_constraint_us(msg_length); // get the time the packet arrived on the UART
uint64_t uart_us = port->receive_time_constraint_us(msg_length);
uint32_t now = AP_HAL::millis();
uint32_t dt_ms = now - _last_ms; uint32_t now = AP_HAL::millis();
_last_ms = now; uint32_t dt_ms = now - _last_ms;
_last_ms = now;
// round to nearest 50ms period
dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms; // round to nearest 50ms period
dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
// work out an actual message rate. If we get 5 messages in a
// row with a new rate we switch rate // work out an actual message rate. If we get 5 messages in a
if (_last_rate_ms == dt_ms) { // row with a new rate we switch rate
if (_rate_counter < 5) { if (_last_rate_ms == dt_ms) {
_rate_counter++; if (_rate_counter < 5) {
} else if (_rate_ms != dt_ms) { _rate_counter++;
_rate_ms = dt_ms; } else if (_rate_ms != dt_ms) {
} _rate_ms = dt_ms;
} else { }
_rate_counter = 0; } else {
_last_rate_ms = dt_ms; _rate_counter = 0;
} _last_rate_ms = dt_ms;
if (_rate_ms == 0) { }
// only allow 5Hz to 20Hz in user config if (_rate_ms == 0) {
_rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200); // only allow 5Hz to 20Hz in user config
} _rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200);
}
// round to calculated message rate
dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms; // round to calculated message rate
dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
// calculate pseudo-itow
_pseudo_itow += dt_ms * 1000U; // calculate pseudo-itow
_pseudo_itow += dt_ms * 1000U;
// use msg arrival time, and correct for jitter
uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us); // use msg arrival time, and correct for jitter
state.uart_timestamp_ms = local_us / 1000U; uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
} state.uart_timestamp_ms = local_us / 1000U;
} }
}

2
modules/mavlink

@ -1 +1 @@
Subproject commit d2cc7dbff67f8c318a40e1ef57a99488b4737fab Subproject commit ff8ada062c96f04dc830e54b657aca4430408433
Write Preview
Loading…
Cancel
Save