mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-07 08:28:30 -04:00
73be185414
moved ground start to first arming added ground start flag moved throttle_integrator to 50hz loop CAMERA_STABILIZER deprecated - now always on renamed current logging bit mask to match APM added MA filter to PID - D term Adjusted PIDs based on continued testing and new PID filter added MASK_LOG_SET_DEFAULTS to match APM moved some stuff out of ground start into system start where it belonged Added slower Yaw gains for DCM when the copter is in the air changed camera output to be none scaled PWM fixed bug where ground_temperature was unfiltered shortened Baro startup time fixed issue with Nav_WP integrator not being reset RTL no longer yaws towards home Circle mode for flying a 10m circle around the point where it was engaged. - Not tested at all! Consider Circle mode as alpha. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2966 f9c3cf11-9bcb-44bc-f272-b75c42450872
298 lines
6.7 KiB
C
298 lines
6.7 KiB
C
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#ifndef Mavlink_Common_H
|
|
#define Mavlink_Common_H
|
|
|
|
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
|
|
|
byte mavdelay = 0;
|
|
|
|
|
|
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
|
{
|
|
//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC);
|
|
if (sysid != mavlink_system.sysid){
|
|
return 1;
|
|
|
|
// Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem
|
|
// If it is addressed to our system ID we assume it is for us
|
|
//}else if(compid != mavlink_system.compid){
|
|
// gcs.send_text_P(SEVERITY_LOW,PSTR("component id mismatch"));
|
|
// return 1; // XXX currently not receiving correct compid from gcs
|
|
|
|
}else{
|
|
return 0; // no error
|
|
}
|
|
}
|
|
|
|
|
|
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
|
|
{
|
|
uint64_t timeStamp = micros();
|
|
switch(id) {
|
|
|
|
case MSG_HEARTBEAT:
|
|
mavlink_msg_heartbeat_send(
|
|
chan,
|
|
mavlink_system.type,
|
|
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
|
break;
|
|
|
|
case MSG_EXTENDED_STATUS:
|
|
{
|
|
uint8_t mode = MAV_MODE_UNINIT;
|
|
uint8_t nav_mode = MAV_NAV_VECTOR;
|
|
|
|
switch(control_mode) {
|
|
case LOITER:
|
|
mode = MAV_MODE_AUTO;
|
|
nav_mode = MAV_NAV_HOLD;
|
|
break;
|
|
case AUTO:
|
|
mode = MAV_MODE_AUTO;
|
|
nav_mode = MAV_NAV_WAYPOINT;
|
|
break;
|
|
case RTL:
|
|
mode = MAV_MODE_AUTO;
|
|
nav_mode = MAV_NAV_RETURNING;
|
|
break;
|
|
case GUIDED:
|
|
mode = MAV_MODE_GUIDED;
|
|
break;
|
|
default:
|
|
mode = control_mode + 100;
|
|
|
|
}
|
|
|
|
uint8_t status = MAV_STATE_ACTIVE;
|
|
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
|
|
|
mavlink_msg_sys_status_send(
|
|
chan,
|
|
mode,
|
|
nav_mode,
|
|
status,
|
|
load * 1000,
|
|
battery_voltage * 1000,
|
|
battery_remaining,
|
|
packet_drops);
|
|
break;
|
|
}
|
|
|
|
case MSG_ATTITUDE:
|
|
{
|
|
//Vector3f omega = dcm.get_gyro();
|
|
mavlink_msg_attitude_send(
|
|
chan,
|
|
timeStamp,
|
|
dcm.roll,
|
|
dcm.pitch,
|
|
dcm.yaw,
|
|
omega.x,
|
|
omega.y,
|
|
omega.z);
|
|
break;
|
|
}
|
|
|
|
case MSG_LOCATION:
|
|
{
|
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
|
mavlink_msg_global_position_int_send(
|
|
chan,
|
|
current_loc.lat,
|
|
current_loc.lng,
|
|
/*current_loc.alt * 10,*/ // changed to absolute altitude
|
|
g_gps->altitude,
|
|
g_gps->ground_speed * rot.a.x,
|
|
g_gps->ground_speed * rot.b.x,
|
|
g_gps->ground_speed * rot.c.x);
|
|
break;
|
|
}
|
|
|
|
case MSG_NAV_CONTROLLER_OUTPUT:
|
|
{
|
|
//if (control_mode != MANUAL) {
|
|
mavlink_msg_nav_controller_output_send(
|
|
chan,
|
|
nav_roll / 1.0e2,
|
|
nav_pitch / 1.0e2,
|
|
nav_bearing / 1.0e2,
|
|
target_bearing / 1.0e2,
|
|
wp_distance,
|
|
altitude_error / 1.0e2,
|
|
0,
|
|
crosstrack_error);
|
|
//}
|
|
break;
|
|
}
|
|
|
|
case MSG_LOCAL_LOCATION:
|
|
{
|
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
|
mavlink_msg_local_position_send(
|
|
chan,
|
|
timeStamp,
|
|
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
|
|
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
|
|
(current_loc.alt - home.alt) / 1.0e2,
|
|
g_gps->ground_speed / 1.0e2 * rot.a.x,
|
|
g_gps->ground_speed / 1.0e2 * rot.b.x,
|
|
g_gps->ground_speed / 1.0e2 * rot.c.x);
|
|
break;
|
|
}
|
|
|
|
case MSG_GPS_RAW:
|
|
{
|
|
mavlink_msg_gps_raw_send(
|
|
chan,
|
|
timeStamp,
|
|
g_gps->status(),
|
|
g_gps->latitude / 1.0e7,
|
|
g_gps->longitude / 1.0e7,
|
|
g_gps->altitude / 100.0,
|
|
g_gps->hdop,
|
|
0.0,
|
|
g_gps->ground_speed / 100.0,
|
|
g_gps->ground_course / 100.0);
|
|
break;
|
|
}
|
|
|
|
case MSG_SERVO_OUT:
|
|
{
|
|
uint8_t rssi = 1;
|
|
// normalized values scaled to -10000 to 10000
|
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
|
mavlink_msg_rc_channels_scaled_send(
|
|
chan,
|
|
10000 * g.rc_1.norm_output(),
|
|
10000 * g.rc_2.norm_output(),
|
|
10000 * g.rc_3.norm_output(),
|
|
10000 * g.rc_4.norm_output(),
|
|
0,
|
|
0,
|
|
0,
|
|
0,
|
|
rssi);
|
|
break;
|
|
}
|
|
|
|
case MSG_RADIO_IN:
|
|
{
|
|
uint8_t rssi = 1;
|
|
mavlink_msg_rc_channels_raw_send(
|
|
chan,
|
|
g.rc_1.radio_in,
|
|
g.rc_2.radio_in,
|
|
g.rc_3.radio_in,
|
|
g.rc_4.radio_in,
|
|
g.rc_5.radio_in,
|
|
g.rc_6.radio_in,
|
|
g.rc_7.radio_in,
|
|
g.rc_8.radio_in,
|
|
rssi);
|
|
break;
|
|
}
|
|
|
|
case MSG_RADIO_OUT:
|
|
{
|
|
mavlink_msg_servo_output_raw_send(
|
|
chan,
|
|
motor_out[0],
|
|
motor_out[1],
|
|
motor_out[2],
|
|
motor_out[3],
|
|
motor_out[4],
|
|
motor_out[5],
|
|
motor_out[6],
|
|
motor_out[7]);
|
|
break;
|
|
}
|
|
|
|
case MSG_VFR_HUD:
|
|
{
|
|
mavlink_msg_vfr_hud_send(
|
|
chan,
|
|
(float)airspeed / 100.0,
|
|
(float)g_gps->ground_speed / 100.0,
|
|
(dcm.yaw_sensor / 100) % 360,
|
|
g.rc_3.servo_out/10,
|
|
/*current_loc.alt / 100.0,*/ // changed to absolute altitude
|
|
g_gps->altitude/100.0,
|
|
climb_rate);
|
|
break;
|
|
}
|
|
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
case MSG_RAW_IMU:
|
|
{
|
|
Vector3f accel = imu.get_accel();
|
|
Vector3f gyro = imu.get_gyro();
|
|
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
|
|
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
|
|
mavlink_msg_raw_imu_send(
|
|
chan,
|
|
timeStamp,
|
|
accel.x * 1000.0 / gravity,
|
|
accel.y * 1000.0 / gravity,
|
|
accel.z * 1000.0 / gravity,
|
|
gyro.x * 1000.0,
|
|
gyro.y * 1000.0,
|
|
gyro.z * 1000.0,
|
|
compass.mag_x,
|
|
compass.mag_y,
|
|
compass.mag_z);
|
|
|
|
/* This message is not working. Why?
|
|
mavlink_msg_scaled_pressure_send(
|
|
chan,
|
|
timeStamp,
|
|
(float)barometer.Press/100.,
|
|
(float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw
|
|
(int)(barometer.Temp*100));
|
|
*/
|
|
break;
|
|
}
|
|
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
|
|
|
|
case MSG_GPS_STATUS:
|
|
{
|
|
mavlink_msg_gps_status_send(
|
|
chan,
|
|
g_gps->num_sats,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL);
|
|
break;
|
|
}
|
|
|
|
case MSG_CURRENT_WAYPOINT:
|
|
{
|
|
mavlink_msg_waypoint_current_send(
|
|
chan,
|
|
g.waypoint_index);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
|
|
{
|
|
mavlink_msg_statustext_send(
|
|
chan,
|
|
severity,
|
|
(const int8_t*) str);
|
|
}
|
|
|
|
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
|
|
{
|
|
}
|
|
|
|
#endif // mavlink in use
|
|
|
|
#endif // inclusion guard
|