Made a repo: https://code.google.com/p/rpicopter/source/browse/
And no I am not 350 X sending 32 at once. Every time I send a new 32 byte command.
The only reason I can imagine, is that maybe one byte is not read out and kept in the buffer.
import socket
import json
import serial
from time import *
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('0.0.0.0', 7000))
# whether to use or not to use this timeout is the question?!
#ser = serial.Serial('/dev/ttyACM0', '115200', writeTimeout=0.1)
ser = serial.Serial('/dev/ttyACM0', '115200')
#chksum calculation
def chksum(str):
c = 0
for a in str:
c = ((c + ord(a)) << 1) % 256
return c
#main loop for receiving a json string
#The checksum will get validated on every important step
def main():
while True:
# Wait for UDP packet
data, addr = sock.recvfrom(128)
fchksum = int(data[data.find("*")+1 : len(data)])
data = data[: data.find("*")]
# Do nothing if checksum of incoming message is wrong
if chksum(data) != fchksum:
print "Wrong checksum! Received: %d, calculated: %d" % (fchksum, chksum(data) )
continue
else:
# parse it
p = json.loads(data)
# if control packet, send to ardupilot
if p['type'] == 'rcinput':
str = "%d,%d,%d,%d" % (p['roll'], -p['pitch'], p['thr'], p['yaw'])
# calc checksum
chk = chksum(str)
# concatenate msg and chksum
output = "%s*%x\r\n" % (str, chk)
print "Send to control board: " + output
ser.write(output)
# flushInput() seems to be extremely important
# otherwise the APM 2.5 control boards stucks after some commands
ser.flush()
ser.flushInput()
ser.flushOutput()
main()
#ifndef DEFS_h
#define DEFS_h
// Number of channels
#define APM_IOCHANNEL_COUNT 8
// PID array (6 pids, two for each axis)
#define PID_PITCH_RATE 0
#define PID_ROLL_RATE 1
#define PID_PITCH_STAB 2
#define PID_ROLL_STAB 3
#define PID_YAW_RATE 4
#define PID_YAW_STAB 5
// Motor numbers definitions
#define MOTOR_FL 2 // Front left
#define MOTOR_FR 0 // Front right
#define MOTOR_BL 1 // back left
#define MOTOR_BR 3 // back right
// Radio min/max values for each stick for my radio (worked out at beginning of article)
#define RC_THR_MIN 1000
#define RC_THR_MAX 1800
#define RC_THR_80P 0.8 * (RC_THR_MAX - RC_THR_MIN) + RC_THR_MIN
#define RC_YAW_MIN -180
#define RC_YAW_MAX 180
#define RC_PIT_MIN -45
#define RC_PIT_MAX 45
#define RC_ROL_MIN -45
#define RC_ROL_MAX 45
uint32_t LSTPKT = 0;
int16_t CHANNELS[APM_IOCHANNEL_COUNT] = { 0,0,0,0,0,0,0,0 };
char COMBUF[32];
int COMBUFOFFSET = 0;
// PID array (6 pids, two for each axis)
PID pids[6];
// ArduPilot Hardware Abstraction Layer
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
// MPU6050 accel/gyro chip
AP_InertialSensor_MPU6000 ins;
#endif /*DEFS_h*/
float wrap_180(float x) {
return x < -180 ? (x + 360) : (x > 180 ? (x - 360) : x);
}
//checksum verifier
uint8_t verify_chksum(char *str, char *chk) {
hal.console->printf(str);
hal.console->printf("\n");
uint8_t nc = 0;
for(int i=0; i<strlen(str); i++)
nc = (nc + str[i]) << 1;
long chkl = strtol(chk, NULL, 16); // supplied chksum to long
if(chkl == (long)nc) // compare
return true;
return false;
}
// Parse incoming text
inline
void ParseInput(int &bytesAvail) {
for(; bytesAvail > 0; bytesAvail--) {
char c = (char)hal.console->read(); // read next byte
if(c == '\n') { // new line reached - process cmd
COMBUF[COMBUFOFFSET] = '\0'; // null terminator
// process cmd
char *str = strtok(COMBUF, "*"); // str = roll, pit, thr, yaw
char *chk = strtok(NULL, "*"); // chk = chksum
if(verify_chksum(str, chk)) { // if chksum OK
char *ch = strtok(str, ","); // first channel
CHANNELS[0] = (uint16_t)strtol(ch, NULL, 10); // parse
for(int i = 1; i < APM_IOCHANNEL_COUNT; i++) { // loop through final 3 channels
char *ch = strtok(NULL, ",");
CHANNELS[i] = (uint16_t)strtol(ch, NULL, 10);
}
LSTPKT = hal.scheduler->millis(); // update last valid packet
memset(COMBUF, 0, sizeof(COMBUF)); // flush buffer after everything
}
else {
hal.console->printf("Invalid chksum\n");
memset(COMBUF, 0, sizeof(COMBUF)); // flush buffer after everything
}
COMBUFOFFSET = 0;
}
else if(c != '\r') {
COMBUF[COMBUFOFFSET++] = c; // store in buffer and continue until newline
}
}
}
void setup() {
// TOO LONG FOR FORUM
}
void loop() {
static float yaw_target = 0;
// Wait until new orientation data (normally 5 ms max)
while (ins.num_samples_available() == 0);
// serial bytes available?
int bytesAvailable = hal.console->available();
ParseInput(bytesAvailable); // Parse incoming text
// turn throttle off if no update for 0.5seconds
/*if(hal.scheduler->millis() - LSTPKT > 500) {
CHANNELS[2] = RC_THR_MIN;
}*/
long rcthr, rcyaw, rcpit, rcroll; // Variables to store radio in
// Read RC transmitter
rcthr = CHANNELS[2];
rcyaw = CHANNELS[3];
rcpit = CHANNELS[1];
rcroll = CHANNELS[0];
// Set an upper limit for the settable throttle (80% of maximum)
// to allow to counter regulate if copter is changing angle
/*if(rcthr > RC_THR_80P) {
rcthr = RC_THR_80P;
}*/
// Ask MPU6050 for orientation
ins.update();
float roll, pitch, yaw;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll) ;
pitch = ToDeg(pitch) ;
yaw = ToDeg(yaw) ;
// Ask MPU6050 for gyro data
Vector3f gyro = ins.get_gyro();
float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);
// Do the magic
if(rcthr > RC_THR_MIN) { // Throttle raised, turn on stablisation.
// Stablise PIDS
float pitch_stab_output = constrain_float(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
float roll_stab_output = constrain_float(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
float yaw_stab_output = constrain_float(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);
// is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
if(abs(rcyaw ) > 5) {
yaw_stab_output = rcyaw;
yaw_target = yaw; // remember this yaw for when pilot stops
}
// rate PIDS
long pitch_output = (long) constrain_float(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
long roll_output = (long) constrain_float(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
long yaw_output = (long) constrain_float(pids[PID_ROLL_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);
// mix pid outputs and send to the motors.
hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);
}
else {
// motors off
hal.rcout->write(MOTOR_FL, RC_THR_MIN);
hal.rcout->write(MOTOR_BL, RC_THR_MIN);
hal.rcout->write(MOTOR_FR, RC_THR_MIN);
hal.rcout->write(MOTOR_BR, RC_THR_MIN);
// reset yaw target so we maintain this on takeoff
yaw_target = yaw;
// reset PID integrals whilst on the ground
for(int i = 0; i < 6; i++) {
pids[i].reset_I();
}
}
}